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ABSTRACT 


An application of two-dimensional Fourier Transform correlation 
for the attitude measurement of three-dimensional objects is considered. 
Included are geometric optical equations and geometric translation and 
rotation equations necessary for the image error calculations. Mini- 
mum attitude resolution equations are developed to estimate image 
quantization necessary for a given measurement error. These techniques 
are then applied as an additional measurement to the radar tracking 


problem. 
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T. INTRODUCTION 


The development of sophisticated naval weapons has resulted in the 
requirement for high accuracy measurement tests and discrete analysis of 
complex system parameters. Typical of this precise test and evaluation 
requirement is attitude measurement of three-dimensional airborne objects 
by processing multiple two-dimensional images. 

The most common method used to estimate attitude is by making 
absolute or precisely relative three-dimensional geometric trajectory 
measurements of target object extreme structural features (nose, tail, 
right wing tip, left wing tip, etc.) from camera images, By appropriate 
camera, range coordinate system, and target coordinate system trans- 
formations these measurements will yield an attitude measurement. 

A more direct method of attitude measurement involves the correlation, 
or comparison, of an image of an object at an unknown attitude with a 
second image of a similar object at a known attitude. At the Naval 
Weapons Center, an operator controlled film and television correlation 
assessment technique (FATCAT) facility use this more direct technique 
for attitude estimation [1]. 

This thesis presents an enhancement of the correlation/comparison 
method of attitude measurement by fully automating the technique incorp- 
orating a pattern recognition method of the directly digitized image. 
Image theory, resolution, and filtering leading to the two dimensional 
matched filter attitude measurement concept is discussed and this concept 


is applied as an additional measurement to a radar tracking problem. 





II. IMAGE THEORY 


Three-dimensional to two-dimensional projections are obtained by 
viewing the real world as recorded by a camera or other imaging device. 
Image theory defines projection as the transformation of each point in 
the field of view, through a focal point, and onto a plane. This is 
also equivalent to locating the focal plane in front of the focal point 
and changing the sign of the projected points [2]. These geometrical 
relationships are illustrated in Fig. 1. The transformation of point 
x, Ys z in the X, Y, Z coordinate system of Fig. 1 is given by the 


following equations: 


ic 
u = X ie) 
vant 
aP 
V = Z (2) 
soe 


Where u = projected x dimension 


V = projected z dimension 


If f << y, then the following approximations hold: 





fx 
omy 7s (3) 
Fz (4) 
= —— k 
V y y 





Therefore, for a given focal length f, and target range y, a constant 


image Scaling factor k, is calculated. 
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Camera Geometry 


A second coordinate system X', Y', Z' can be used to describe a 
three-dimensional object in the viewing space. If the origins of these 
two coordinate systems are coincident, and if all the axes are parallel, 
then the transformation of points from one coordinate system to the 


other would be simple. 
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If the axes of the two coordinate systems are rotated by an elevation 
angle 9 around the camera Y axis, by an azimuth angle ¥ around the camera 
Z axis, and by roll angle ¢ around the camera X axis, the transformation 


equation would be [3], [4]: 


x! cos@ QO sine}{cosy -siny Of} 1 0 0 X 
y'| = 0 1 0 Sint cosw O||0 cos -sing y| (6) 
Za -Sing 0O cosé 0 0 1} /0 sing COS ¢ Z 


If in addition there was a translation of (A,B,C) of the origin of the 
object coordinate system relative to the origin of the camera coordinate 


system, the transformation equation would be: 


i} 
cD 
— 
-o- 
oe 


(7) 


where 
cos6 Q  siné 
‘Oe 0 


“sing O cosé 


= sin cos¥ 0 


0 0 ] 


] 0 0 
0 cos -sind 


| cosy -Siny 0 


QO sing cos 9 


in 





III. IMAGE RESOLUTION 


The three-dimensional object can be considered to be an aircraft, 
with maximum orthogonal dimensions on the focal plane of Xmax, Ymax, 
Zmax. These dimensions are illustrated on the top and side views of 
Fig. 2. Since the image is to be digitaized, the discrete maximum 
dimensions can be given in terms of pixel sampling intervals, x and y. 

A pixel is defined as the smallest unit sampling area (x,y) of resolution 


for the digitized image. 


Xmax(quantized) = XN (8) 
Ymax(quantized) = YN (9) 
Zmax(quantized) = ZN (10) 


These dimensions are illustrated in Fig, 3. The quantization of the 
image would have a minimum rotation that could be detected by a 1 pixel 
change at extreme points in the image and would be equivalent to the 


following angle: 


tang = — (ar) 


This relationship is illustrated in Fig. 4. KN would be equal to XN, YN, 


or ZN depending on the angle of rotation. 
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Maximum Image Dimensions, Orthogonal Views 
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Figure 3 


Discrete, Orthogonal Views 
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1 pixel Pivot Point 
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Figure 4 


Minimum Detectable Rotation, Orthogonal View 


[If the axis of rotation was not orthogonal to the focal plane, the 
minimum detectable rotation of an equal sized object would be much 


larger, aS indicated in the top view of Fig, 5. 


KN=] 





KN 


Sample values of KN from equations (11) and (13) for minimum detectable 


rotation are given in Table 1. 
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TABLE 1 


Axis Orthogonal Axis Parallel 
to image plane (11) to image plane (13) 
9 (degrees ) KN (pixels) KN (pixels) 
] 58 6566 
5 12 263 
10 6 56 


If it is assumed that the rotational angles are measured by lines 
through the two extreme pixels, the geometry and error sensitivity would 
be as shown in Fig. 6. For a maximum length of KN. and a 1 pixel rota- 


felon: 


7] 
tango = —— che 
KN 
For horizontal errors H: 
] 
8 = arctan eee (14) 
KN - H 
] 
ag Sl clipe) =e (15) 
KN - H 
For a change of variables: 
] 
KN +H 
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dm = “(Nt H)e” (17) 
] 
m 
dH 
dg = ————_,~—_ 19) 
cKN  H)o41 


For example, if KN = 58, H = 1, then dé6/dH = 0.0003077 radians/pixel = 
0.01763 degrees/pixel. This sensitivity for horizontal errors is very 
small, as expected. 


For vertical errors: 








1- V (20) 
Q = arctan 
KN 
12% 
m = C218) 
KN 
dV 
dm = —— (22) 
KN 
dV KN 
de© —y—— (23) 
KN" + (1 = V) 


For example, if V = 1 pixel, and KN = 58 pixels, d6/dV = 0.01722 radians/ 
pixel = 0.99 degrees/pixel. This is much larger than the horizontal 


error, aS might be expected by observing the geometry of Fig. 6. 
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One-Dimensional Rotation Errors 
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This section has derived equations (11) and (12) to predict the 
minimum detectable rotation of a three-dimensional object. These 
equations apply to a three-dimensional object with a two-dimensional 
maximum projected dimension of KN pixels. Equation (11) applies if 
the axis of rotation of the three-dimensional object is parallel 
with the camera axis. Equation (13) applies if the axis of rotation 
of the three-dimensional object is orthogonal to the camera axis. 
Equations (19) and (23) predict the rotational errors resulting from 
horizontal and vertical measurement errors in extreme points of the 


two-dimensional image. 
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IV. IMAGE FILTERING 


Two-dimensional matched filtering can be considered to be an 
extension of one-dimensional correlation filtering [5]. The out- 
put of the two-dimensional matched filter will not be a transformed 
image with some type of improvement or enhancement, but a correlation 
plane whose amplitude at a given location corresponds to the degree 
of correlation of the input image (signal + noise) with the desired 
image (signal). This is equivalent to sliding one photographic 
transparency across another and adding up each black point that was 
aligned with another black point on the second image, each white 
point that was aligned with another white point on the second image, 
and each grey point that was aligned with another grey point of 


equal shade on the second image. 


Several practical factors must be considered in the actual 
simulation of the matched filter. The input image and the refer- 
ence or stored image could both be compared (correlated) as contin- 
uous grey scale images, but this would be a tremendous storage and 
computational task. Therefore, the reference image in the computer 
simulation is digitized and stored as discrete X,Y,Z points (FACIT 
Model) in a three-dimensional coordinate system [6]. It is recon- 
structed, after rotation and translation, as unit amplitude lines 


projected on the focal plane. It is also assumed that f << y, and 
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f and y of equations (1) and (2) are known. This knowledge of 
focal length and target range yields a constant image scaling fac- 
tor (k). This type of projection is illustrated in Fig. 7. An 


example of an aircraft FACIT Model is given in Fig. 8. 


The input grey-level image is also transformed into a similar 
format by use of an edge detector to extract outline information 
[7].° The edge of the image is defined as the boundary between the 


two regions of different grey level. 


The reference image and the transformed input image are applied 
to a matched filter to determine the degree of correlation. An 
jdeal correlation function C(x,y,k,9,¥%,¢) would have an impulse 
response tnat could be modeled by delta, (sin x/x), triangle or 
exponential functions. The impulse would occur when the reference 


and transformed images were: 


1. aligned on the image plane (x,y), 
2. equally scaled (k) and 


3. at the same attitude (6,v,¢). 
However, due to the presence of noise and the approximation of the 


three-dimensional object with a finite element model, the correla- 


tion response is not delta, (sin x)/x, triangle or exponential in 
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Figure 7. 


Three-Dimension to Two-Dimension Projection 
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shape. It is not necessary to know the exact correlation response 
function or even the constant component due to a nonzero mean value 
of the reference and input image variable function. It is only 
necessary to know the values of the variables x, y, k,@.y and¢ 


at peak correlation. 


The following simplifying assumptions are made for an attitude 


(€,¥,¢) only correlation: 


1. the images have been scaled to an equal size (k) 
2. the peak correlation on the image plane will indicate 


translational alignment (x,y) 


Fig. 9 illustrates a typical x~-y correlation between a noisy image 
and computer-generated reference image. The peak correlation at 
(0.0, 0.0) pixels indicates that both images are aligned in the 
x-y plane. This is because the noisy image was generated from a 


computer reference image. 


The attitude of the input image is unknown. Therefore, the 
measurement of the attitude of the input image is equivalent to 
applying the transformed image to a bank of matched filters with each 
"tuned" to a specific attitude. This process is illustrated in 


Fig. 10. 
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Figure 9 


Sample 64 X 64 Pixel X-Y Correlation Plane Obtained 
from 32 X 32 Pixel Image and 32 X 32 Pixel Reference 
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Figure 10. Image Processing Flow Diagram 


If there was a priori knowledge of azimuth © , elevation wv , and 
roll ¢ , to within 10 degrees each, and a |] degree resolution of 
measurement was required, a brute force approach would require 
10 x 10 x 10 = 1000 correlations or 1000 matched filters. Three axis 
quadratic interpolation is used to reduce the number of correlations 


required for the given resolution of measurement [8]. 


It is assumed that the variables azimuth, elevation, and rol] 
are uncoupled in the joint correlation function C(x,y,9,% 4%. Assum- 
ing that there is a near orthogonal relationship allows C(x,y,9 sw9) 
to be sequentially maximized (minimized) with the independent vari- 


ables: x, y, azimuth, elevation, and roll. 
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The pitch correlation function is modeled as a quadratic func- 


thon C(@): 





D 
C(o)=aeo +bete (2a) 
ale a. 
2 = 28 bes (25) 
Equation (25) is set to zero to maximize (minimize) C(5). 
= lp (26) 
max oar 


6, = 0 degrees, 85 = 8; + A® degrees, and 63 = 6, - A® degrees are 
Substituted in equation (24) to solve for the unknowns a, b, and c. 


The solution of equation (26) is: 


9 z C(63) - Cio A@ 
max 2c cH eck b= 4 Cie) 


Similar equations are written for C(wv), and C(o). 


The computer simulations listed in Appendix B implement equa- 
ion (27). The three correlations C(6;), C(@.) and C(@3) are calculated 


with Ae = 5 degrees for a rough approximation of ane Then three 


28 





correlations C(91 }, C(82 ) and C(®3 ) are calculated with Aé@ = 2 


degrees for a sharper quadratic approximation of oF In addition, 


ax ° 
the +2 and +5 degree intervals are tested to make sure that the mid- 
point correlation C(91) was greater than or equal to the end point 
correlations C(®2) and C(§3). If either is not, the interval is shifted 
by 2 or 5 degrees in the direction of the larger correlation and the 
mid-point is tested again. This dual pass computation reduces the 
unwanted coupling effect of the attitude variables. The +5 degree 
calculation detects the general angle of maximum correlation. The 

+2 degree calculation reduces the estimated angle error by not being 


as sensitive to unsymmetry and nonlinearities in the correlation func- 


tion. 


A sample of the dual pass azimuth estimation is illustrated in 
Fig. 11. For this sample, noisy edge data of a simulated aircraft at 
an azimuth angle of -45.0 degrees is used as input data. This data 
is correlated with computer-generated edge data from a stored air- 
craft model. The initial estimate of attitude is -48.0 degrees. The 
final azimuth calculated is -44.11 degrees, a 0.89 degree error from 
the true -45.0 degree aircraft azimuth attitude. This is a typical 
maximum error. For example, if the initial estimate of azimuth is 


-42.0 degrees, the final error would be 0.0 degrees. 
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This section has introduced the use of two-dimensional correla- 
tion to determine the attitude of a three-dimensional object. Equation 
(24) is used as a second order polynomial model for the correlation 
function of each attitude variable. This approximation reduces the 
number of computations necessary to determine the peak correlation for 
each attitude variable. This quadratic approximation was demonstrated 
with a computer simulation that tested for the maximum correlation of 
an image as a function of attitude. The simulated image was previously 
generated by adding edge noise to a computer-generated image at a known 
attitude of -45.0 degrees. By starting the correlation algorithm with 


a -3.0 degree offset, the final error was found to be 0.89 degrees. 
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V. MATCHED FILTER EQUATION 


It 1s well known that one-dimension correlation can be calculated 
by a simple multiplication of two functions that are Fourier trans- 


formed [9]. 


eee te x(t) (ter) dy = x(t) * Af=t) (28) 


where t is the dummy variable of integration and * is used to 


symbolize the convolution integral. 


C(w) = X(w) H¥(w) = X*(w) H(w) (29) 


where w = 2nf = frequency in radians and * is used to indicate com- 


plex conjugation; that is, if 


H(w) = R(w) + jl(w), then H*(w) = R(w) - ce 


c(t) <=> C(w) (30) 


where <=> is used to indicate a Fourier transform pair; that is 


C(w) is the Fourier transform of c(t). 
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This same relationship can be shown in two-dimensions and in discrete 








form [5]. 
N-1 M-] 
Sie > fx (4.95) W(k+i,1+j) (31) 
1=0 j=0 
C(m.n) = X*(m,n) H(m,n) (32) 
K-1 L-] 
j 2 mk j Za nh 
C(m,n) = En Kee. K e~ -L 
k=0 1=0 
I-] J-1 
21 mi .2inj 
ee hicneen maul e J 
i=0 j=0 (33) 


variable in first dimension 


ao | 
ll 


variable in second dimension 


= 
tl 


K = J = M = Period in first dimension 


| = J = N = Period in second dimension 


33 





A complete listing of the transformation indicated in equations (32) 


and (33) is given in Appendix A. 


The reason the Fourier domain correlation approach is used is 
that for large data arrays the Fast Fourier Transform (FFT) computa- 
tion time is less than that of direct time or space domain correlation. 
Also, Fourier transform and array processing hardware is available for 


many scientific computers. 


The continuous correlation equation (28) states that an infinite 
integration is necessary to determine the correlation at a particular 
time. This is not the case for finite sampled functions. The summa- 
tion interval for the sampled functions requires that the majority of 
the information in the two functions be contained in the finite sampled 
interval for the correlation to approximate the continuous correlation. 
When x(t) and h(t) are sampled with an impulse function A(t) over a 
finite interval, Cotte = NT, their Fourier transforms x(w)*A (w) 
and H(w)* A(w) will be periodic with period w = 1/T, where T = sampling 
interval of the impulse function A(t). When the continuous function 
x(t) is sampled over the interval O< t ee and Fourier transformed, 
this is equivalent to calculating the Fourier Series coefficients of a 
sampled periodic sequence x, (nT) of period N, where N = oa and 
T = time interval between samples. If x0 (nT) is time shifted in 
relation to x(nT) by m samples, and the first N samples are trans- 


formed, it can be shown that the resulting complex Fourier transform 
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will change in phase but not in amplitude or period [9]. In the 

time domain, this shift of m samples is due to the periodicity 

of Xp (nT) and is referred to as a circular shift of a sequence [5]. 
The sequence appears to be circular because the last sample of x(nT) 
that is shifted beyond the interval N T appears at the beginning of 

the sequence. Likewise, in the two-dimensional correlation there is a 
circular shift in two dimensions when one image x(i,j) is correlated 
with another h(i,j). The two-dimensional periodic or circular sampled 
function is equivalent to joining parallel horizontal and vertical edges 
of each image and rotating one image on each of its axes while the 
other image remains stationary. This two-dimensional circular shifting 
is depicted in Fig. 12. In this figure, image 2 has shifted n samples 
in the horizontal direction with respect to the fixed image 1. Image 2 
has shifted m samples in the vertical direction with respect to the 
fixed image 1. To clarify that image 2 has circularly "wrapped around" 


image 1, the corners are labeled A, B, C and D. 
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Figure 12. Implementation of Two-Dimensional Correlation 
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The continuous images 1 and 2 are sampled at N and M discrete 
points in the x and y axis respectively. This sampling results in 
a periodic equation (33) in the transformed frequency domain of per- 
iod N and M. If the correlation product of the two sampled images 
has nonzero terms beyond the N and M dimensions, there will be an 
overlap of nonzero samples in the frequency domain. This overlap 
phenomenon of a high frequency component taking on the identity of 
a lower frequency component is referred to as frequency aliasing. 
Likewise, spatial aliasing would occur if there was an overlap of the 
periodic spatial functions; for example, if the first and last samples 


added. 


To test the correlation method of attitude measurement, a com- 
puter simulation program, FAC listed in Appendix B, was run. Discrete 
boundary points were generated as input data for a known aircraft 
attitude as illustrated in Fig. 13. A noisy test object was generated 
that was within +5 degrees in azimuth, elevation and roll of the ref- 
erence object. The noisy boundary, as described in the next paragraph, 
was compared with the noise-free computer-generated edge data to esti- 
mate the aircraft attitude. The representative errors of attitude 
(azimuth, elevation and roll) were computed by comparing the attitude 
that was iteratively predicted by equation (27) with the actual atti- 


tude. These results are summarized in fable II. 
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Figure 13 


Boundary Points of FACIT Model 
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Table II. 


Representative Attitude Errors (Degrees) 


Three Sample Average, 1 Pixel Noise Standard Deviation 


Azimuth elie 
Elevation 255 
Rol] O71] 


It was noted that the errors increased with each attitude measurement. 
This iS because of the sequential coupling of a previous attitude 


angle error to the next quadratic angle interpolation. 


The quadratic correlation attitude estimator (27) was initial- 
ized with data to within 10 degrees of the known aircraft attitude. 
Boundary points on a 32 x 32 grid were generated from the two- 
dimensional projection of the test aircraft. The number of boundary 
points generated were a function of the test aircraft attitude, but 


typically 108 points were generated: 


(27 each: maximum horizontal, minimum horizontal, maximum 


vertical, minimum vertical). 
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To simulate detector noise, Gaussian noise with zero mean and one 
pixel standard deviation was added to each boundary point, horizon- 
tally and vertically. Since the boundary point function was on a 
discrete 32 x 32 grid, this had the effect of quantizing the noise 
to unit pixel intervals. For example, if the absolute value of the 
Gaussian noise was less than 0.5 pixel, no noise would be added. If 
the absolute value of the Gaussian noise was between 0.5 and 1.5 
pixels, one pixel of noise was added. Likewise, two or more pixels 
of noise would be added for greater outputs of the generator. These 


noisy boundary points are shown in Fig. 14. 


An uncalibrated system test with real data was made from a 
digitized video image of a F102 aircraft. The intensity data from 
the image was transformed into edge points. The edge points were 
correlated with the model data stored in the computer memory. This 


predicted the aircraft's attitude. 


The x,y resolution of the digitization was 340 pixel columns 
x 220 pixel rows and the intensity resolution was 1 to 256. As the 
photograph in Fig. 15 shows, the aircraft image makes up only a small 
portion of the video image. Therefore, a 64 x 64 pixel window shown 
in Fig. 16 was contrast enhanced, in that the original intensity 
range of 103 to 156 was linearly transformed to a 1 to 256 intensity 


level. It is noted that the digitized image is not displayed at the 
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Figure 14. Boundary Points of FACIT Model with Added Noise 


40 


60. 


a0), 


4O, 


30. 


oO; 


10. 





RO . 
= SEAS 


— 
ESS 
SSS SN 


SS 





Figure 15 
Photograph of F102 Aircraft 
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Figure 16 
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correct aspect ratio due to the unequal sampling ratios taken verti- 


cally and horizontally. 


The edge data from Rosenfeld's Detector [7] was applied to a 
variable threshold iteration that was adjusted until greater than 
200 pixels were above the threshold. This adjustment was necessary 
so that low contrast edge pixels typically at wing, nose and tail 
extremities would exceed the threshold. These extremities are most 
important in obtaining a given attitude accuracy. Fig. 17 shows the 
results of the best computer-generated match of the stored image 
Superimposed on the edge data obtained from the video image. The 


computer predicted the attitude to be: 


Azimuth = -115.0 degrees 


Elevation 45.0 degrees 


Rol] - 5.0 degrees 

There was no other data available to establish the attitude of the 
F102 aircraft at the instant the camera recorded the image. It is 
presumed that further investigation would show this attitude to be 
correct to within the limits of the image resolution and the edge 


noise present. 


43 





VERUMCAL PIXELS 


LEGEND 
*=STORED IMAGE EDGE PIXELS 
Vie EDGE FIxers 


80 
60 ; 

,S° “s e -°*@. @ 
eae Goo coo - 
ome © oO Oucoococcc G2 & 

; ocdoocoooocooca ae 
: ‘9 OO GOOScooCCCCCG oa es) 
40 co a! Q oO coqocopOcooocCCE co o 
ooco ccogcoococcooccnccccocccco aie 
gcoccosoqosccccccccces a - OmeG 
_Q mco- coccco oococcao fs) ,0 09 
<8. OOOO o o * .°9 
20 ; : <= * 
oe tS a ocoococce a 
Bic ie a. ococoocccno I 
: (= Re ea oococo occo * 
| on o .@° 0 ‘ao °**-@.o0qqoo,: 
*0 10 20 30 40 50 60 70 


HORIZONTAL PIXELS 


Figure 17 


Best Fit of Stored Image Pixels to Video Outline Pixels of Fig. 16 
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The lack of sharpness in the transformed edge data shown in 


Fig. 17 can be attributed to several factors; 


T. Low x,y resolution (64 x 64). 
2. Low intensity range (103 - 156). 
3. Uneven lighting and shading on edges. 


4. A simple edge detector. 


The last factor would be the only one that could be significantly 
changed given a free-flying aircraft, fixed camera locations, and 
natural lighting. A review of methods used for extracting features 
based on edges and contours is included in a survey by Lavine [11]. 
More recent theories and experiments on edge detectors have been 


demonstrated by Abdou and Pratt [12]. 


This section has introduced the use of two-dimensional Fourier 
domain correlation as described by equation (32). Representative 
results of three computer correlation simulations were listed. The 
Simulation compared a computer generated edge model to a previous 
computer generated model with added edge noise. Finally, a sample 
correlation of the computer generated edge model with the edge data 
detected from a real video image of an aircraft at an unknown atti- 


tude was described. 
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VI. AIDED AIRCRAFT TRACKING 





An example of the utility of the attitude measurement is demon- 
strated by utilizing an aircraft positional estimating Kalman Filter 
with combined radar and electro-optic sensor measurements [10]. It 
is difficult for the radar-only estimator to keep a close target in 
track if it is highly maneuverable. The criterion of "highly man- 


euverable" for a particular radar is dependent on the following: 


1. The sampling interval of the radar. 


The range gate period, or the sampling aperture. 


CO 


The receiver sensitivity. 

4. The return signal to noise ratio. 

5. The mount azimuth and elevation dynamics, or time constants. 
6. The measurement accuracy of azimuth, elevation and range. 


7. The point source tracking of a finite length target. 


These parameters and others contribute to the ability of the radar 

to track with minimum error. By modeling the aircraft target to be 
driven by an acceleration function that is dependent upon aircraft 
attitude, more accurate range, range rate and range acceleration state 
estimates are possible. The acceleration function is based on the 
well-known aerodynamic coupling between the target attitude and the 
direction of the target acceleration. This coupling is summarized 


in the following comments: 
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T. Major accelerations (lift) are normal to the velocity vector 
and the pitch axis. 

é. Positive lift is more likely than negative lift due to pilot 
physiological factors and to structural loading design. 

3. Accelerations in the velocity direction (drag/thrust) are 
generally smaller in magnitude and of shorter duration than 


the lift accelerations. 


A complete multiple measurement system using the previously discussed 
imaging attitude measurement concept as a radar tracking aid to the 
positional estimating Kalman Filter is shown in Fig. 18. In the upper 
left corner of Fig. 18 there are n radars that provide range, azimuth 
and elevation measurements to the Iterated Extended Kalman target 
estimator. In the lower left corner there are m video cameras that 
input data to m correlators. These correlators first scale the stored 
model to a size equal to the predicted input image size. The scaling 
is a function of target .range and system gains such as telescope mag- 
nification and image pixel sampling area. The correlators then apply 
equation (33) as described in the previous section for the computer 
simulation FAC. The attitude measurements azimuth, elevation, and 
roll, from the correlators, are transformed into a common coordinate 
system. The predicted attitude of a model aircraft in a coordinated 


turn is combined with the m attitude measurements in a Kalman Filter. 
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Attitude Measurement and Tracking System 


Figure 18. 
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The estimated attitude from the Kalman Filter predicts an accelera- 


tion vector that is used as an input to the target estimator. 


The simulation of a target range (R) estimation was performed 
with the program KAL listed in Appendix B. The simulation KAL was a 
limited example of the radar tracking filter as illustrated in Fig. 18. 
This simplification was made possible by not including some tracking 
variables, such as azimuth and elevation, to the full three-dimensional 
trajectory filter. The trajectory was truly three-dimensional, hence 
there were changes in azimuth and elevation, but they were not used 


as system measurements or estimated as parameters. 


The Iterated Extended Kalman Filtering subroutine FTKALI jis a 
modification of the IMSL Kalman Filtering subroutine FKALM [14]. 
The system was modeled as a radar track of a nominal constant accel- 


eration target. The assumptions for this trajectory are: 


1. The average acceleration rate is zero. 
?. The acceleration rate between radar scans 1s constant. 


3. The acceleration between scanning intervals in uncorrelated. 


The above assumptions are described in the well-known Alpha-Beta- 
Gamma Filter equations [13]. Written in a discrete form, these 


equations are: 


49 








ey te 2 nad 6 na] (34) 
| w. 

meee | * n= * 2 (35) 

een it T are] (36) 


where ar is a random sequence with zero-mean and a mean-square 


acceleration rate of 


ar | =e ae aq) a Ie (37) 


QO otherwise 


The target trajectory was simulated in three parts: 


1. A target constant closing velocity of 100 or 175 
yards/second at the same altitude as the radar. 

2. A 10-g target turn in the azimuth plane of the radar. 

3. A 10-g target turn “up" in the geometric plane perpendicular 
to the azimuth plane and the ground projection of the radar 


range vector. 
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The final, or third turn, starts when the target has rotated through 
90.0 degrees of the 10-g turn in the azimuth plane. The maneuver 


was started at three initial ranges: 


ime Oistant : iZ7s.0evards 
2. Mid-range 1000.0 yards 
3. Close-in 150.0 yards 


Inputs to the filter of two noisy radar range measurements and an 
optional radial component of the predicted normal acceleration vector 
were generated at a sample rate of 10/second. A one sigma standard 
deviation Gaussian noise of 1.0 yard was added to the two range 


measurements. These inputs were input to the range filter. 


Typical results of range error for one simulation of all man- 
euvers are listed in Table III. The addition of the predicted normal 
.acceleration measurement to the radar range measurements reduced the 
peak and RMS errors for all maneuvers. The reduction in error was 
most dramatic for the close-in maneuver because of the range reversal 
for the particular azimuth and elevation geometries. At the mid- 
range and distant range trajectories there was no range reversal 
so the additional acceleration measurement was less helpful in cor- 
recting the range and range rate estimates In addition, the high 
radar sample rates and the averaging effect of using two radar measure- 
ments reduced the difference in error between the radar-only estimate 


and the combined estimate. 
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Range 
Maneuver 
1. Distant 
2. Mid-Range 
3. Close-In 


Error 


Tabje III. 


(RMS Average/Peak - in Yards) 


Combined Measurement 
(Normal Acceleration 


and Two Radars) 


DET O/ S422 
0589/4525 


4.83/7.56 


Two Radars Only 


O59) 4-62 


106/509 


10520712702 


Fig. 19 shows the aircraft 10-g turn trajectory plotted in three 


dimensions for a 175 yards/second air speed. 


As compared to the 100 


yards/second air speed trajectory shown in Fig. 20, the distance 


traveled down range is much greater, and the turning radius was much 


larger. However, within the 5 second interval of flight, the air- 


craft in Fig. 19 did not climb as high as that in Fig. 20. This is 


because the third maneuver (turn up) did not start until approximately 


3 seconds versus 2 seconds for the 100 yards/second trajectory. 
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Figure 19 


Three Dimensional Aircraft Trajectory, 
Velocity = 175 yards/second 


a6 





200 


\00 


SENT SUMC INE = ADS. 


TARGET TRAJECTORY 


LEGEND 
SiS Ome Onl 
0 = GROUND PLOT 
Ae— ELE VY Oe ANCE 





Figure 20 


Three Dimensional Trajectory, 
Velocity = 100 yards/second 
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In Fig. 21 the radial error and the measurement noise in yards 
is plotted versus time for the close-in maneuver. The radial error 
is equal to the true radial distance minus the estimated radial dis- 
tance in yards. Gaussian noise with zero mean and a standard deviation 
proportional to the square root of the estimated radial range variance 
was the measurement noise added to the true range. For this example 
the noise has a 1 sigma standard deviation of 2.0 yards. The very 
large peak error at 0.8 seconds is due to the aircraft flying very 
nearly "overhead". This then reverses the sign of the relative 


welioeity vector. 


The velocity reversal is shown in Fig. 22 where X(2) is the Kalman 
estimator velocity state, and VR2D is the first difference of range 
divided by the sampling interval. The slow time constant of X(2) is 
due to the lack of a velocity measurement. A more accurate velocity 
estimate is shown in Fig. 23 for the distant turn maneuver. There is 


no velocity reversal in this trajectory. 
Fig. 24 shows the range acceleration state estimate, X(3), and 
the measured range acceleration, Y(3). The two accelerations track 


quite closely and describe two phenomenon: 


1. An initialization. 
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Range Acceleration Estimate and Range Acceleration Measurement 
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2. Cyclic acceleration matching the period of the third 


part of the maneuver. 


The acceleration state estimate in Fig. 25 with no acceleration 
measurement does not show this periodic acceleration, indicating the 


uncertainty present in the state estimate. 


This section applied the correlation attitude measurement tech- 
nique to the radar tracking problem. The attitude is related to 
acceleration and therefore a useful measurement in predicting target 
acceleration. Recursive trajectory equations were used in a computer 
Simulation that estimated aircraft range. The simulation demonstrated 
that the Kalman Filter was more accurate in tracking a target with 
varying trajectories when the predicted acceleration was used as in 


addition to the noisy range measurements. 
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Range Acceleration Estimate Without Range Acceleration Measurement 
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VII. CONCLUSIONS 


Manual optical image comparisons have been the primary method 
of measuring attitude of three-dimensional objects. Section V lists 
the general two-dimensional Fourier transform equations that are 
applied to an images edge data and a stored model of the solid ob- 
ject. The attitude of a three-dimensional object is estimated by 
using a three-axis quadratic interpolation to sequentially maximize 
the correlation output as a function of the attitude, position and 
size variables. Moreover, this measurement or estimate is equivalent 


to the maximum output of a bank of matched filters. 


This concept was then applied to realistic Kalman Filter track- 
ing simulations and shown to give improvement over conventional target 
state estimators that do not use the attitude measurement as a pre- 


dictor of target acceleration. 
Since the necessary attitude measurement is within the electro- 


optical tracker and computer state-of-the-art, the use of this esti- 


mator concept can improve present tracker system capabilities. 
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APPENDIX A 
TWO-DIMENSIONAL DISCRETE CORRELATION 


This appendix demonstrates that the two-dimensional discrete 
correlation transform (31), (32) is equivalent in both the time domain 


and the frequency domain. 


N-] N-] 

Oke) =..'s fF Gl al) eee oes cele (31) 
i=0 j=0 

C(m,n) = X*(m,n) H(m,n) (32) 


where * = complex conjugate 
Equations (31) and (32) are verified by performing the Fourier Transform 


and conjugation operations indicated. 








M-] N-] -J m -—j—dJjon 
a= 2D 1 X*(m,n) e oe 2 : 
1=0 j=0| MN m=0 n=0 
(A-1) 
ZR 2m 
a j —plkti) j ——q (143) 
Melee 2) (pq) e M N 
PQ p=0 q=0 ae 
om oll 
) © We RS Sy a a ee 
e(k,1) = rr C«“ Cw X*(m.n) H(p.q) e @ 
m=0 n=0 p=0 q=0 
(A-2) 
21 2 I 21 = 
Noe at a er ee ee pee’ LL 
| ye? 2 & : 
MN  i=0 j=0 
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The term in brackets in equation (A-2) is equal to: 
la 7 ; _ - 
TIN (MN) = 1, if m= p andn=q 


0 > if m# p andnF#q 


due to orthogonality of i and j. Therefore: 








21 2M 
: 1 M-] N-1 e ‘ m k j n ] 
m=Q n=0 


(A-3) 


‘Yt . Le(mon) ¥(n.n)} 
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APPENDIX B 
COMPUTER PROGRAMS 


This Appendix is composed of the listings of the Fortran programs 
ind subroutines for the attitude measurement and target tracking 
‘imulations. The programs were run on a Univac 1110 computer with 
| Fortran compiler and a FLECS translator (Fortran Language with 
‘xtended Control Structures), The graphical outputs were plotted 


ising DISSPLA programs that generated COM-80 crt images. 
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Cm GL 


MOgn 


Belurs=5. 


NL=C 
CHAXC7I=ACHAX (4) 
WAS =<¢ 

NAY=yY 

ELACTI=REL 


ELACZIZ=ELMADELErS 
CLACSI=HEL*DELERS 


S59 CONTINUE 


DO 4ul JehAGeNAY 
JMzJ-c 


Ge NERATcC IMAGE cpOUNDARY FROM S3=DIRENTIONAL MOVEL 
whR = Cakb = OUTPUT VcCTOR CINCYM X INCYS) 


CALL SUKFACCAZ,ELAC IM) -ROLL eYMAX eZMAXYMIN eZMINI es IPLOTeIPRINT ) 
CALCULATE CORRELATION BETWEEN INPUT IMAGE AND GENERATcCO IMAUVE 3 QUNDA 


CALL CALCORCKK eed CeNV eAZCELACIH) ROLL ee NIZ N22) 
GG CONTINUE 

NLENL¢1 

IFC WL eGT.26) 60 T0 «2Cle 

WRI TE(6.419) 


mgr OR PUAN C7, 5X5 J 2 Ske “ELACUM) 70 7X 0° OAK ( I), 2) 


DO 411 J=7.69 
JMEJ=6 
WRITE CO631712) Se tLAC IM) -CMAXCJ) 


411 CONTINUE 


TEST FOR MAXIMUM CORRELATION. ITF CH¥AX(C7) ¢ CMAXCK) OR CMAKCY) 
CORRELATE AT ADDITIONAL POINTS 


IFO CMAXO7YLTeCMAX(&)3 GO TO 4141 
IFC CMAX(7),.L Te CMAX(9)) GO TO 4142 
GO TO 4144 


G141 TFC CMAXR(CE) LT oCMAX(9)) GO TO 4142 


ELAC3)=ELAC1) 
ELACTISFELAC 2) 
ELAC2ZISELACZI=“DELErS 
CMAX(G)=SCMAX C7) 
CMAXC 7) =CMAX C5) 
CMAX (8) =-12E10 

NAS=6 

NAG =6 

GO TO 390 


414¢c CONTINUE 


ELACZ)=ELAC1) 
ELACTISELAC3) 

ELA CSISELACZI4F+VELEPS 
CMAX(E)=CMAX (7) 
CMAXC7) =CMAX CG) 
CMAX(9) =-172-E 10 

NAY =Y 





SC. ey 


™ 02 & 


G. tt GG» 


WAd ay 
Sera u S590 
~144 CCNTINUeE 


CALCULATE “OST PROcABLE ELEVATION FOR PNPUT FRAME CY GUDKANRIC WT 


ELS TSOCMAXO3ZV=CHAX (C9) RDELEPS S02, HC MAXXIS) 47 .eCMAX CC) —Ge eC MAK (1)) 
PELSTSLIFEXCELST + eS) 4IT FIX CELAC1)) 
Pkeawl selec ST.LEL st 

em AT C76 SX 6° ELST =", F12.5.5Xe°TELST =°.1b0/) 


REDUCE INTERVAL OF INTERPULATION IF AUS. vALUc OF ELST > 1. 


Mee peoece oT si feisu) GU TO 6202 
NAS =64 i 
NA =Y 
ELA CCISELACTI=“LELERS 
ELACS)SELACT)4DELEFRS 
Chak ( d6=—-1.616 
CMAa ( 9d=-12E10 
60 TO 396 
Getic CONTINUE 
EL=FLUATCIELST) 


RUTATE MODEL IN RULL 


DEL KHC=5. 
NLEC 
CMAXCT3ZI=CHAXC7) 
ROLLACT)=EROLL 
ROLLACZ)=ROLL=-DELRHO 
ROLLACS3)=ROLL*DELRAO 
NA14=14 
NA15=15 

490 CONTINUE 
00 SCO J=NA14,NA15 
JM=J-12 


GENERATE ITMAGE SOUNDARY FROM 3-DIMENTIONAL MOVEL 
WHR = CARD = OUTPUT VECTOR CINCYM X INCY™) 


CALL SURFACCAZ EL se KULLAC IM) .YMAX LMAXLYMIN eZMINeJS eo ITPLOTSIPRKINT ) 
CALCULATE CURRELATION BETWEEN INPUT IPAGE AND GENERATED IMAVE Z3OUNDA 


CALL CALCORCKK2 edu ce eNV eALe EL eKOLLACIM) eV et T20eN22) 
S0u CONTINUE 

NL=ANL 41 ; 

PeecriesG1.«¢) GO TG 3202 

witeletec or 5 10) ; ; 
ep RGnIA TT CY, 5X. J. SK. ROLLACIWD 7 7X6 ° CMAK (J) ~./) 

fom tt J=35-4.15 

JM=Jjeid 





CCC ee 


 ) 


7] 


>161 


ara’ c 


md 


>, 


wv 
(ams 
fV 


al 


Wieletc CG. 5 1c) 
CONTINUE 


JemOLLAC IM) .CMAXK (CE) 


Tc ST FOUR MAXIMUM LURRELATION. 
Meee kATE AT ADULTIONAL FUITNWTS 


IF 


ees X 442) .L1T.CMAx« (14)) GO 
IFC CMAXC15) -LT.CmAKA(15)) GO 
Ee 1O 5144 

TFC CmMAXCTO) LT eChAR C199) 
ROLLACSISRILLA( 1) 
ROLLACTISAOILLAC.) 
ROLLACZYEROLLACCI=LELKHO 
Sr Ax tts) =€4AxX(15) 
CMAXCTEV SCAR C74) 
CRAXC14)S-1,.¢1U0 
NALT4L=H14 

NAT5=14 

GO Tu 4Sbt 

CONTINUE 

ROLLA 2YEROLL ACH) 
ROLLACTI=AKOLLAC 3) 
ROLLACSIFROLLAC JI *UELKEHO 
CMAX(CT4)=C4AX( 45) 
CMAXOET3%YEC¥%AR (15) 
CMAXC15)s-1.c140 

NA14=15 

NATS=15 

60 TO &9€ 

CONTINUE 


TO 
TO 


GQ TO 


CALCULATE MOST FPROGABLE 


CKRAX CTS) 


ROLL FOR INPUT FRAME 28Y 


€ Cyr Ax(14) 


suUBDRATIC 


ROLLST=H(CRAX C14) =-C MAX CTS) #®DELRHOSOC 2. OC MAX C15) 420% CMAX( 146) 


mG. eCMAKC1)) 


IROLSTHIFIXCRULLST#e S)OTFIXCROLLA( 


LROLST 
ere fre as) oe 


PRINT $323.ROLLST. 
COU FORMATC/.5X-° RULLST = 


1)) 


IROLST =°.14./) 


RECUCE INTERVAL OF INIERPOLATION IF AupSe. wALUc OF ROLLST > 1. 


Peterson GlLieST).-LT.ete) GO FO 5272 
DEL RKRPU=HCc. 

NA14=14 

NAT5=15 

ROLLACZIFRILLACTI“VELHKHO 
KCLLACZI=HARILLACTI*#ACELArO 

CMA OCT4QIFe-F er 1VU 

CMAXCT5)ES-1-°E Tu 

GC Tu 69t 
CONTINUE 
I°’LOotze 
PeeCPAS Ts ues 0) 
STUF 


GOrsty 7 


7) 


UO kK ChAx(15 ) 


PNT Er 





a 


<OCO TVDUYV 


a » 


oO fF Geri aa vv CG 


cCENER 


MANN CK KONA OOMN DM L& 


— 
nN 

La 
nh 


Slee AL 


aC KOTATES &A STUKED 3D IiMewTIGhAL MODEL In 3 ARTIS ALv*UTMeELEVAT= 
The FRGJECTcev TRACE UF The MODEL CN A FLANe SURFACe IS GUANTI2Zc0 
wm (32x%e¢) CKIO. OUTPUT ArnrnwAY = “CARD” 


Loh GUTINE SURKRFACCAZe cL eROLL oe VRAXTeZMAXTSYMINI SZ MINTS ASITPLOT, 

IPR INT) 

AnAMETER NCS=71K4=2 "NCS 

RRAMETER INCYM =52 

PRAMETER IZE=CeIE=1 

AnAMETCR NI=Z5eNU=OeNOPH=7 

IMENSTON AKPAXCINCYM) cP C3 ea eNCS)eTh 505 eNOS) eXNORMC ICS). YKC2 NCS). 
NORP (NCS) .ZNORM(NCS) »pAKMINCINCYM) « oKMAXCINC YM ), 
EKMINCINCYM ) 

IMENSTON CARD(160304) 

IMENSION YLC710) -2L¢10) 

CMMON CARD 

CMMON/FACDATS P 

ERU=UeV 

NE=14. 

CTR=) 

RCHACOSCTHT-I/ 100. 


AL FREPERATION OF DATA 


ZR=AZ*eL RC 
LRFEL*® URC 

CLE R=RULLADRC 
AZ=SINCAZR) 
AZ=CUS €AZR) 
cCL=SINCELR) 
cL=COSCELR) 
ROSSINCFOLLA ) 
RO=COS (ROLLEI) 
MAX=-4U.E10 
FINZSTOU2E10 
PAX=-1G0.2E170 
MIn=z1O02.E1€ 
Carag © 1%-215e1cF 10.2) 


E TARGET 


G 10 K=1,.NCS8 

0 1G J=1.4 : 

(VedeKDEP CL ede KIMLELHCAL HPL eed eK BCEL SAL OP C5 od KD ASEL ; 
(Ze FeKV=EPCT ed eh V*ECCRUMSAZSSRONRSELHCAL APOC Seda KI ACC ROMCAT“SnO*SEL* 
AZ -PCoedIeK I *#SHOXCEL 7 

CE,d eK )EPCT eden ®CSRUSAZA“LCROMSELACAL PP Cee de KI ACC RORSELSSAL 
Toomoere Are P(t. Je KD AC RUSCEL 

Pelee J —ekK Youll e¥r Ar) YMAX=TO 2d 5K) 


ez 





mimer (aed eK) eLT+¥h IN) Viewty 1 Cc kD 
Mor Cs ed eK) eGTeLMAX) ZMAX=HTI Sed eK) 
Pt gd pK YeLT.ZhINY EMINZETO 3,4 6K) 
CNT TINUE 


mens) YN=-- Awd 2¥-- VALUCS 
CTRFLCTR*4 

m@meewe. 1) GU TO te 

PAX T=YRAX 

rPINT=aYmIh 

Maa T=ZP Ax 

pINV=aZin IN 

GNTIWNUE 


Te SURFACE NORPALS 


mer 15 K=1.NC 5 

x MP ODER) T0252 0K) SCT C3, 1.KI-T OS ob KID MCT 2s TKD 
TC 2eheK ACTS tT KITES ye eK %)d 

3 1) C326) H CT (TT KI AT OT 9 KD) HCFCS NOR) 

Mase ye OTC 1.1,K)-TC1.2-K)) 

PN eT tate KKT OC 1e2 KDI ACTOZ DHKD ATOZ one KID ETE Te 10h D 

TOVebeK DI #CTOSe tT oKIA“The otek) 

C UNTINUE 


‘OMPU TE ILYMAX AND IZMAX 


CTR=LCTR?1 
ZNAX=ADSCTM AX=-ZMIN) 
YMAX=ABSCYMAX-YMINY 
ZMAX=RZMAX 
YMAX=RYMAX 


PREPARE PLOT PAGE 


FCIPLOT-EQeC) o6 To #19 

LiL OTMWPPLITCS) 

XL THS AMAXTOCZRAX -ZMIN y, CyMAxk -Y¥MIN V) 
XLGTH = AINTCAALGTH/ 2.) 4 16 
CHTREAINTCCYMAX*YMIND Je) 
CNTKEAINTCOCZMAX*ZMIN)D JO) 
CORESACNTR-AXLGTH 

UCRGEZYCNTR-AALETE 

ENDSACNTR*AXLGTFE 

END SYCNTR*AXLGTH 

ALL uGWPl f-1) 

ALL NOWRDR 

ALL FEIGRT(C.1) 

RCODE CODe2TLeLALELIAZL EL RULE 

ALL TITLE CL ADEL 1076” go eAeee "Aly Erle GO 
Pare CRAFUXORG 5 eKEND se YURG o> eVEND? 

mig ta RE RC LD 

ALL eet ric owt) 





NO OM OE tii 


L! 
— 
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| oe 
BTAnT 
‘af 
D 
D 
+] F 
us 
em F 
K 
K 
5 
I 
a 
D 
Z 
rINo 
cE 
A 
A 
oi 60OCOF 
Aw 
D 
Y 
I 
N 
0 
K 
I 
I 
f 
1 
x 
rs) 
I 
fs iN 
[ 
l 
I 
I 
l 
p 
ay5 1 
) 
1¥4 C 
He LC 
com 
ot) 


ONT Th@t 
IwebexX FOR ~e-KADTER 


nhCY*SFLOATCINCY®) 

LY=KYMAX/RENC YD 

LZAFKZITRAX/RNCYM 

LKMATC ? Pelle Dieled ee Gites Yael ies” san Mies Pet @ eh orks IU te) 6 LO Ke 
“DELIA” SXe"1LZMAN ee TKS LT YMAX? OS) 
LKMATCOFIS. 5) Aare ae ae 

rap = 

rIH=U 

ccD=1e 

Pec b= 7 

mene (Oe 1222 ) LCTR 

Geite [Z=t~eIncyr 

CAR=IZ*®OLZA*ZMIN 


Y INTENSECTIONS 


VE=L 

KFAX€TZ=-70.E1U 

KMINCIZ)=1G6.2E1C 

CRMAT( ” Ye ZEARe AKMINCIZ) » AKMAX (IZ) eDYMINe ZYMIN CINCY) -DYMAX. 

MUP IMACINCY) 0° 220Xe°IZels IFUGoTAKFLGOCENCY) »INCY® of) 

C 40 JT=1,NCS 

KCT eI =YMIN-1UCG. 

FOXNORMC(I) LE eU+) GO TO 4d 

bk=6 

0 25 w=1.4 

c=K 41 

F(x eEQe4) KE=1 

OWES C165 .K I] )—TC3.KEs1T)) LE 106-6) GO TO <5 

TO eet) -<TC2 eke 1) )*PCLZBAR MKT C3 eKe LIDS C TCS KEG] U-T C3, KID)? 
TCG eK eI) 

Ce eT) TC 1 Kel) 28 CZ2GAnHWTC3eKe LIDIA CT CZ. KE eT -TU5,K.100+ 

i A ere 

NCY=CY-YMINIJ/OLY 


TEKSECTION wITHIN cOUNDS OF TRE SURFACE 


MavmoeTCaek eI) cANDeY¥sbTeT (2eKEeT)) GO TO g5 
Penny (2ekel) ANDY et FeTf2eKE+1)) GO TO 25 

F(Z EAR eLT eT CbeK eI eANDoZUAN oL ToT (SeKE II) OO ro <> 
FIZEAR eGToT (234K 91. eAND eo LBAN GT oTIZ,KE el) uv O TO 25 
FCY LE eAKMAX(I2)) GO TU 195 

KMAXCIZI=Y 

PCeGE sAKMINCIZ)) GO Tu ye 

KMINCTZ)=Y¥ 

OMT INUE 

CXNTINUE 

CNT INUc 

CGATIhUVUe 





FLO I 


Qf 
55 
109 


ey ee ee ae a ae 


START 


Lu rAKULOM NCISE TG AkMNw-( 2) 


Peryts YL CI) 


Meer lseQse.’ uv TO «5 
LCILI=FZLAR 
LC LIZA AR 
HONCAKMAXCT 2) eh Te PL TL eUK eAKRMAXACIZ) GT oPLTg) 
UNLESSEAKPINGIZY LTePLT1.GR eAKMINOIZ) oo TePLTO? 
e YECVIZARKRDPINCGIZ) 
° KMINEK RING J 
e CALL Gurve CYL, ZLs 1.71) 
ooo F IN 
oof IN ‘ 
CS 
a@KENCAKMINCIZ  eL Te PLT 1.0m eAKMINO IZ) eG ToPLI2) 
° YLEVISAKMAKCIZ) 
e KMAX=S=KMAX?4 7 
eeoeFf IN 
ELSE 
© YULCTISARMAXCI2) 
© YLC2Z)SAKMINGIZ) 
° KiPmAX=K MAX? 1 
e KMIN=K MING 1 
@ CALL CURVE CYL-ZL. conti) 
seer LN 
eof IN 
GOs TINUc 
GwTInUec 
UNTINUE 
CTR=LCTR*1 
PIN =sU 
tAX =U 


eee’ FOR Y—-KASTER 


L vwAa=0 


x ©C £ 


FIND 


=n KA Cm OM 


MIN =U 

RITEC6.4222) LCTk 
O 50U £ =1, INCYS 
tAw=I «OLY +YH IN 


Peimrer SECT ichs 


KRPAKCI Fete cE Ve 

KMINCI YStuectde 

oeGbu Jaton CS 

FOXNORM OY Ler ee)? VO THU 4bu 

G iLO R=1e0S 

E=x 9] 

F(x e924) KE=1 

mre T (cK )-TlceKEod))eL Eo 1eE 0) vy U To ser 


“a 
U1 





Seo —-roRlLZORTAL LIMyTS FOn SEWUFWTIAL ¢ SCAR 
See J=VERTICAL LIMITS FUR ScQUENTIAL Y SCAN 


ER JmAGE wiItik RESPECT T0 FIRST ImAGE ANU USE GRIv INTERVALS UF 
FoIm SCAN IMAGE GUTTOM TO TOP (1) AND LEFT TO RIGHT CJ) AT 
KETE INTERVALS. 


EKT MAX, AND PIN’ EDLE COONDINATES TO GRID INTEGER De 


EY IT N=YRIN=“V¥rIn ] 

EYMAKZYMAX=YMAX 1 

ELy=€ pEYMIN*DEYMAXIY/2, 

PIN=YMINV4D0 ELY 

LYTHACSCYMAXIT-“YMINTI/ARNCYD 

cZMIN=ZMIN=-ZMINI 

EZMAX=LMAX=ZMAX1 : 

ELZ=( DEZMIN*DEZMAXI/ Ce 

MIN=ZMINT4#D ELZ 

LZAT=AGSOZMAATHAZMINIDAKNC YM 

NCTREG 

G 717050 IT=1.INCYe 

R= CKRMAXCT) -YMIN) JOLY 1 

X=LRFIXCFXY 

X=(CAKMINCT) -YMIN) JOLY 1 

G=IFIx(GxX) 

G CHARACTER FLAG 

kKF=G 

CWhITRKAST PIXEL FLAG 

t Fa. 

G 100% J=4, INCYM 

YOCERMAXOC J) -ZEIN)D /OLZA1N 

FHoIFIXCFY) 

Y=(EKMINCJ) -ZMINI/OLZAIN 

O=IFIX(GY) 

R s( y=) )e INC YM4 

FCIF NED) GO TO 1679u 

kK=24 

kRF=1 

ONTINUE 

F(CITX Need) GU TO 1033 

CaK DC TR J=FONE 

EF= 1 

C TO 144 

ONTINUE 

CARDCIRJHFTZEKC 

BelGwNews)) GO TC 1: 

CAKDCIRY=ONE 

tF= 14 

NCTRSINCTR? 1 

GO TO 144 

CNT INUE 
CAKOCITRKI=HZE RU 

| Ween’ 61) GY TO 136u 
CAKCCIRKIVEONE 

lLeFs* 


Lo 
Lad 
CO 
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imeem nNE oJ —-TC Spe JI aC YH RR-T 2 eke JIS CT (2 eKEe J) -T2ekeJ))°* 
Pees eK g J ) 


b> IN Tes SECTION wlThIn sOUNUDS UF ThE SURFACE 


ett st CS AGS) AND CZF LT eT i ekted)) vO To 393 
eae TES eRe JV CAND ZF AGT.TOC iy E,dY) GO TO 209 
reenact eT Co eh oJ) CAND eC VYOAR eGTeTCcekE J) ) 60 TD SGU 
Fiera r el Tel (cek oJ) -AND. YEAR at} ee. Khe ae ime OF To ore 
fimetebceBPKYAX(T 9) GU TO 2.0 

sArAXCT J=ZF 

LPeRASL MAX? 1 

Cchn]TINUE 

Peer sot eH kKMIN(l )) GO To 235 
bArIACI JZF 

Lirln=aLMIN 4 
C 
C 
e 


a 


o> CNT IWUc 
oC UONTINUet 
6UC UNTINUE 


ADO KANUCM NOISE TO 8kM--(I <2) 


PEOY FUINTS 


IFOLPLOT.EG.C) GO TO 4&5 

YLCcCI=AY GAR 

YLQCVI=FRAYUAK 

@KREN CO K MAX CT ) LT ePL TS eUR oe EKMARCT )GTORLTS) 





©o UNLESSCRK MING] DeLTePLT3.0R eEKMING I )eGTePLTG?) 
° © LZLCVIFcKMINCGY ) 

e e JMINSSI&PING 1 

° © CALL CURVECYLe2Le te) 

e eoeF Lh 

e eet IN 

Eek 

® wrcN (BK KINGI Veal lari l3.U0R.eKMIN C1 »eGTePLT4) 
eet PS ERMAXCT ) 

° © JSMAXSIRAXt47 

e ° CALL CURVE CYL ZL « 3471) 

r AP ae ae 

° fon eS = 

° ° ZELCII=REEKRMAXCI ) 

. © “LuCZI=ELKMINCGT ) 

° ° JimAx=ShRAKSO 1 

. ° JP iNn=Jr In 1 

a e CALL CURVE CYLeZL-. Zenmi) 


° seer LN 
woth 1 ia 
me 6 Cd 6 Uf 1c 
mo. CC ChilwJec 


erGisnt tiv U & 
LCT stC rae 





var 








Cro 164% 

ONT UINUe 

CAnDCTRIVSEZE RY 
mesceNtel) GO TG 1745 
CAnOCIRI=ZAONE 


Ter=} 


nNDwDrAT AO Ven OTA ANY 


m7 


G TO 1C44 

ONTINUc 

CAROCIAIVFZE KU 

CNTINUE 

Car iInue 

CNT ITWUe 

CTREFLCTR? 1 
POCIJPRINTsE@ et) CGO TO 11 
Peneieec oes Ad sEL + ROLL 
aLie ENODPL(Q) : 
CTKRAFLCTRK+1 

ONTINUE 

CTKR=LCTR*¢ 4 

ETUkW 

OkMAT(CTH1. 


SN AT IEMUTHEANGLE= ch Gee wts Oo” 6 


“ELEVATION ANGLE="eFdede/.aXe “ROLL ANGLES". FEec ess) 


NO 
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SUDFULTINE VuOuCLOARIN, AKOT eI NEM eNUD) 


VECURKL CUNVERTS An INEUT ONAN) AWAY [NTO A ConX2N) AnkAY Hy aDdDDING 
MACKGRUUND OATA, 


ARINFINPUT ARRAY, wnkKOTH=HCUTPUT ARRAY se INDM= INPUT DIMENTION 


CO OO es ie a 


DIMENSION ARINCINUE). AFUTCNOD) 
NOOCSc* INUM 
NOD=4*1 NU 
FOMP=FLOATCINU®™) 
ROM=SGRTCFO™) 
IOR=IFIXCRO™) 
IOR c=calth : 
el FORPAT(cX.° IUR = ely? } 


Cc 


Cc 


J=RUw INDEX FOR OUTPUT ARRAY LT=COLUMN INDEX 


amb) 


OO cu J=1.1bR2 
TFC J +«GTeIDR)I GO TO 2e7C 
LJ=IVR2elJS-1) 41 
TJUK=ITOR#(J-1)71 
AROTCIJI=ARINGIJK) 

204 CONTINUE 


Pee L REMAINDER UF kROw WITH ZEROS 


OF es 


mee 00 2oU T=1,TvR 
TJ=ITuRe2e(J-TI*L OR! 
AROTCIJ)=0. 

Poo CONTINUE 
GO Tu 3hG 

gc? CONTINUE 


c FILL REMAINDER UF kOwSs wITR ZEROS 


00 ¢coG 14ie1IdRe 
ITJ=LuR2*(J-1)41 
AROTCIJS)=be 

c&U CONTINUE 

$0u CONTINUE 
RETURN 
END 


12 





THIS SUBROUTINE CALCULATES THE CORRELATION SE TWEEN 
THE A VECTORCDFT COEFFICIENTS) AND THE B VECTURCDFT COEFFICIENTS) 


CY es 


SUBROUTINE CALCOR (KK 2 eJSZeNV eAZ ec EL ROLL ed eNI2eN22 ) 

DIMENSTON WH( 16384) -WHR( 16384) 

DIMENSION $(125),INV(128),m(3) 

DIMENSION CMAX(20) 

DIMENSION MAXX(20) MAXY(20) 

DIMENSION w(128,120? 

COMPLEX*8 A16564) ,6(1634%4) 

COMMON wWHReWH,A,CMAX 

M(1)=KKe 

M(2)=J3J/2 ‘ 

4(3)=0 
COST FORMATO ASX 79S =" ole 3X eo CMAX =° 4g F176 /) 
Os) FORMATCSX. N12 =°.14,°N22 ="sI5e°NV =7%2 14) 


C 
C OOQUBLE SIZE OF DATA ARRAY : 
C WHR = INPUT VECTOR. WH = OUTPUT VECTOR. NV = INPUT DIMENSION 
C 

NV4=4aNV 

CALL VOOUBL (WHR gWH,NV NYS) 
C 
C PLOT DATA ARRAY 
C 
C TRANSFER THE WH (K) DATA TO FHE COMPLEX 8B MATRIX 
C 


NOATA=0 

00 104 I2=1,.N22 

DO 103 11=1.N12 

IWH= (IT 2-1) #N12+IT1 

BCI WH) =wWH CIwH) 

IFC wHCIwH) 2GTeO.U) NOATA=NDATA* 1 
103 CONTINUE 

IWKM=I]WK=<-9 

WRITE (6,72) CWHCI), IT=IwWHM.IWH) 
104 CONTINUE 

WRITE(6.208) NDATA 
200 FORMAT(” NOATA =°.17) 

IFC NDATALEQeU) STOP 





C 
C COMPUTE THE DISCRETE FOURIER TRANSFORM OF THE wH (K) DATA 
C 
71 FORMAT(16(1X%,F7e2)) 
72 FORFAT (1601X%,E172-5)) 
| 73) FORPAT(C7°02X-15)) 
CALL FARM(COeMsINVeS el eA ITPERR) 
C 
: C COMPLEX MULTIPLY 
C 


DO 220 11=1.NV4 








na AL 


aA an 


m™) 


rT 
a) 


40 


4] 


4¢ 


BCI VI=ACIT 1) * CONJG(H(1T1)) 


IFCIV eLTeN2c) wRITECO.101) I11.AC11),8(11) 
CONTINUE 


INVERSE FUURIER TRANSFORM 


CALL HARM(BM,INV.S -~1,IPERR) 


CALCULATE THE MAGNITUDE OF THE FOURIER COMPONENTS 
AND STORE ONLY THE POS FREQ VALUES OF w(I1.I2) 


DO 3U I2=1,N22 

IWwR= (I 2-1) *#N12+11 
w(IT,I2=CABS (BCI wH)) 
TECwCILTe1T 2) eL TeCMAX(J)) GO TO 39 
CMAXCJI=WCIT-I 2) 

MAXX(J) =] 1 

BMAXY(J) =I] 2 

CONTINUE 


PRINT THE DFT COEFFICIENTS 


IFC RAXX(J)-6T23) 60 TO 40 
MM3=1 

MM2=2 

MM1=3 
MAXX( J) =4 

MPT =5 

mP2=6 

MP3=7 

MP4G=65 

GO TO 42 
CONTINUE 
NI2FPENT2—-4 
IFC MAXX(J)-GTeN12M) 60 TO 41 
MMS=MAXX( J) =-3 
MM2=MAXX(JI—-2 
MMYT=XOMAXX( J) —1 
MPT=MAXX( JD 74 
MP2=MAXX(J)42 
MPZS=MAXXCJ) 43 
MPG=MAXX(J) 44 
60 TO 4&2 
CONTINUE 
MM3S=NI2—-7 
MM2=N12-6 

MMT =N12—-5 
MAXX(JI=ENIZ 4% 
MPYT=N1i2-3 
MP2=Ni2-2 
MP3=N12-1 
MPG=N12 
CONTINUE 
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wRETECG 6100) MMS SMM2.MMTSMAXX (J) MP1 MPO pM PS MPS 
JOD eRORTAT(” KO Sk a Ae 3. eK) tl Oke Belo. ot) (2) 
1O4V FORMATCOC2X.14,80141X,t42.5)) 
00 61 I2=1,Nce2 
WRITECO.TUIII2, (WC L161 2) e1L1TEMMS MPS) 
61 CONTINUE 
RETURN 
END 
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KAL IS A RADAR RANGE ESTIMATOR USING A KALMAN FILTER. 


THERE ARE 2 RANGE MEASUREMENTS AND A RADIAL ACCELERATION ESTIMATION 
MEASUREMENT. 


PODEL 
XOK97) = HOCK OXCK 46 (CK) aU CK) 
YOK+1) = SOK+T) ONC K41) OV 0K41) 
KALMAN ESTIMATE 
X7(K*1) = HOCK) @X-HAT(K) 
K~HATOKOF) = KOCK V=K CK ODI (CS (KOT) OX? CDK O44) ?d 
KOKI) = P°CKHDI ASTER +T I ACS CK HT APCKD) OSTCKMT) ER (KO7) Deent 
P~ (KAT) = HOKI#PCK) HT CKI+G CK) #Q(K) GT (KH) 
PCK*+1) = P°(CKH1)-KCKO1) 2S OK 91) PK OK 41) 
P(G) = 6(0)2*0(0)#6T(0) 
T = TRANSPOSE 
SUBROUTINES USED: INTCON 
MANVR 
FIKALI 
DIMENSION X04) ,H04,4) ,6064),803 54) oP 06 54),7166 54) 126654) ,T3C3), 
DIMENSION TP(100) ,EP1¢4100),x°106100) ,RNOIS(100),vP1(100) ,vP2(100), 
*®RNOIS2¢100) ,xTP(100),27TP (100) 
DIMENSION AP1°0100),AP2(100) ,aP3(100) 
DIMENSION WHEN(2),TMEODC 2) 
DIMENSION PAUV(3) ,ANUV( 3) ,ANL (2) ,ANLR(3),TPAC3) 
DIMENSION TOPT(S),STATC(S) 
DIMENSION IPAK(15S0) 
COMMON XT,LYT ZT »VXTVYT,VZT VELOC ,TIANG,TZANG,DELR,IG,TRATI, 
eDELTeNPRF,TRATZ,TIeNPH, ROO, 162 e-I1CTSeoPIlLeIR,IN,IOS 
COMMON/INTC/ AZEL eROLT,ROLC,ROLS ,AZDEG, EL DEG ,ROLIDG .ROL DE, 
*ROLGOG ,TESTTES2»RT»,ROOTT,R2DOTT, OT», TRADT,»TRADCRANGT, 
TRANGTM XR eVRoeZReVAXTM VV TMSVZIMeVR2D gAXT,AVT AZT ,AXTG AYTG ALTO, 
®ANA,ANEPAUV 
DATA IN, N,IT/3*4/ 
DATA IS ,41/2*3/ 
DATA ILeL/2et/ 
DATA ISEED/42573/ 
DATA PI/32-741592654/ 
C 1/08 222) MODEL STATE VECTOR 
CALL FRBOID(% 6220 ReHe WILLIAMS 6262 “~) 
NI=5 
NO=6 
TEST=10. 
TGS2=10.- 
07T=0.05 
NP=31 
NPM=NP=-1 
NPH=NP/10 
RES=0.5 
ROO=PI/2. 
R270=3.*PI/lee 
6Y0=10.7252 
NRUN=0 
10 CONTINUE 
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3-D TARGET TRAJECTORY DATA 

ORIGIN AT INITIAL TARGET POSITION 

RT = TRUE RANGE RELATIVE TO RADAR 

ROOTT = TRUE VELOCITY RELATIVE TO RADAR 
R2DO0TT = TRUE ACCELERATION RELATIVE TO RADA® 
VELOC = TARGET VELOCITY RELATIVE TO ORIGIN 


TRAD= = TURN RADIUS OF TGS= Ge TURN 
TRAT= = TURN RATE OF TGS- Ge. TURN CRAD/SEC)D 
-R RADAR LOCATION AT 1T(0) 


-7T TRUE X,Y,2 TARGET POSITION 

V-T = TRUE KMeV¥eZ VELOCITY 

A-T = TRUE XoV¥oZ2 ACCELERATION 

A-TG = TRUE X.Y¥,Z ACCELERATION (6°S) 
ANA = VERTICAL CA7) ACCELERATION RELATIVE TO TARGET ORIENTATION 
ANE = HORIZONTAL CEL) ACCELERATION RELATIVE TO TARGET ORIENTATION 


PAUV(CI) = PITCH AXIS UNIT VECTOR 
PAUV(I) = xX 
PAUV(2) = 2 
PAUV(3) = Y 


ATC = TIME CONSTANT OF ACCELERATION 
TCK = TRANSITION MATRIX ACCELERATION TIME CONSTANT 
ATC = Lis 
TC = ATC/DOT 
TOK = CT EXP C-TC)) 
Q@ = COVARIANCE MATRIX OF U CLXLD =- INPUT 
= €fU*UTJ = CRMS RANGESSEC*#SECBSEC) 222 
READ(5,150) Q,RCV,ILAST 
D0 115 10S=1.e2 
IFCIOS.2EQe.2) b1T=0.1 
00 118 IR=1,3 
00 116 1h=1 92 
H = STATE TRANSITION MATRIX CNXN) 
DATA H/1620./ 
AG@TK = GAMMA ACCELERATION TRANSITION MATRIX CONSTANT 
ARTK = NORMAL RADIAL LOAD ACCEL >’ TRANS. MATRIX CONSTANT 
IFCJHLEQ.4)2 AGMTK=1.0 
IFCIHEQ22) AGMIK=0.0 
IF CIH.EQ23) AGMTIK=0.5 
IF CIHAEQL1) ARTK=0-20 
IFCIH.LEQ.2) ARTK=1.20 
IFCIHeEQS3) ARTK=0-5 
H(1,1)=1.- 
H(1,2)=0T 
H(1,3)=AGMTK*OT*0T/2. 
H(C1,G4)=AaRTK*ADT*OT/2. 
H(2,2.=1 6 
H(2,3)=D0T*AG*TK 
HC 2,4) =ARTK*O0T 
H(3,%3)=1. 
ANLRK = RADIAL NORMAL LOAD ACCELERATION TRANSITION CONSTANT 
ANLRK=T. 
H(4,46)=ANLRK 
G = STATE NOISE INPUT MATRIX (NXL) 
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300 FORMAT (% Q ae on en es R =" @ tO RT =o Ee ote RDOTT =e 
a4(F7.1),13) 


CALL DATE (WHEN) 
CALL TOFDAY(TMEOQD) 
WRITE (NO.112) WHEN(C 1) ,WHEN(C2),TMEOD (1) -TMEOD (2) 
112 FORMAT(C2A6,2X,2A6) 
WRITE (6,90) 
90 FORMAT(”% eG NaoR]  ~ gris ¥ 7s 3 
MAK X01 7K K C2972, “ERROR=ERT-X(1) 792K e NOISE 
2K,“ AXTG~ p2K,°AVYTG~ 2X, “ALZTG%g2Ke°2ND DIFF-VELOC”,/) 
WRITE C6,130) KeRT,¥C190X01)9,X(2),EP1¢ 1),RNOIS(1) 


C MAIN LOOP 


DO 100 II=1,NP™ 
IP=11+1 
IPS=0 
IFCLILJEG.1) YPS=t 
PCTNS=0.7 ‘ 
RNOISCIIV=SQRTCR(1.1)) *®GENOFCISEED) *PCTNS 
RNOIS2Z(TIV=ZSQRTCRC2,2) ) *GGNOFCISEED) *PCITNS 
DELR=VELOC®DELT#®FLOATCNPRE) 
MANVR CALCULATES TRUE POSITIONS (XT,YT,27) AND TRUE VELOCITIES (CyxT, 
VYTeVZT) TARGET STARTS 9&8 ORIGIN 3B T°O, AND FLIES TOWARD RADAR FIXED 
LOCATION RT ON X AXIS : . 
DOWN RANGE, REFERENCED TO FARGET 9 T =0 
UP RANGE, REFERENCED TO TARGET @ T = 
OFF RANGE, REFERENCED TO TARGET @2 T =0 
CALL MANVR 
C V = TOTAL TRUE VELOCITY RELATIVE TO COORD SYSTEM , T(0) 
VISQRICVXT#VXT+VYT*#VYT#eVZT*VvZT) 
AXT=CVXT-VXTMISCOELT® FLOAT CNPRE) ) 
AYTHCVYT-VYTMI/SCDELT® FLOAT CNPRE)) 
AZT=(VZT-VZTM)/SCDELT#® FLOAT CNPREF)) 
C AT = TOTAL TRUE ACCELERATION RELATIVE TO COORD SYSTEM , TO) 
AT=SQRTCAXTAAXTHAYTRAYT#AZT*# AZT) 
AXTG=AXT/10.72582 
AYTG=AYT/10.7252 
AZTG=AZT/10.7252 
ANE=CCAYTEVXTHVYTRAXT) OVXTHCAZTEVYTHVZITAAYT tv Z TIS Ove) 
ANAZCAZTAVIZT-VZT#AXT)SVH 
C AH = HORIZONTAL ACCELERATION ~— RELATIVE TO COORD. SYSe, TCO) 
AH=SQRTCAXTRAXT+AZT#AZT) 
ELE=ANE*+COS CEL) 
ELN=SQRTCANAPANAPELE*ELE) 
C VH = HORIZONTAL VELOCITY ~ RELATIVE TO COORD» SYSe-e T(0) 
VH=SQRTCVXT#VXT*VZT#V¥2ZT) 
AZ=ATANZ(VITOVXT) 
AZDEG=AZ*180./P1 
EL=ATANZ(VYT,VH) 
EL DEG=EL*180./P] 
ANAEL=ANA/SELE 
C 1CTS = COORDINATED TURN SWITCH 
IFCICTS cEQ@e1) ROLS=ATANZ(ANAL1027252) 
ROLS =ATAN2(ANA,1027252) 


x 
Y 
Z 
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GCT)I=FDT*OT#eOTs/4, 
GC2=OT*OT/2, 
G(3)=07T 
6(4)= 6(3) 
R = COVARIANCE MATRIX OF Vi (CMIXM1) - MEASUREMENT 
DATA R/G*#0./ 
RCT,1)=RCV 
R(2,2)= R(1,1) 
R(3,3)=100*R(1,1) 
S = OBSERVER MATRIX (M1IXN) Y= §X 
DATA $/12*0./ 
$(1,1)=1. 
$(2,1)=1. 
INTCON INITILIZES TRASECTORY VARIABLES 


CALL INTCON 
TX = FJTERATEO EXTENDED THRESHOLDS 
TxCTD=1. 
TxC2)=7x (1) 
TXC3)V=TxXC2) 
TxX(46)=TX3) 
K=Q0 
T=Q. 
LL=0 
INCP=Q 
RNOISCII=ASQRTCR(1,97)) *66NOF CISEED) 
RNOIS2C PI=SQRTCRC2,2)) *GENOFCISEED) 
Y = INPUT OBSERVATION VECTOR (1X1) 
MULTIPLE MEASUREMENTS 
YC1T)=RT*RNOIS (1) 
YC2]I=ERT*RNOIS2(1) 
Y(3)=0. 
PRESET ESTIMATOR INITIAL STATE 
X(1) = RANGE(K) 
X(2) = RANGE RATE(CK) 
XC3) = RANGE ACCELERATION 
X(€6) = ESTIMATED RADIAL NORMAL LOAD ACCELERATION 
X1T00F F= 1 
X20K=1.1 
X01) =YC1)4xX100F F 
Xx(2)=X20K *RDOTT 
x(3)=0. 
¥(4)20. 
€P1( 1)= RT-xX (1) 
XP1¢07)=xX 01) 
VP1¢01)=x C2) 
VP2(1)=RDOTT 
ap1¢01)=x3) 
ApP2(1)=0.- 
ap3(1)=0. 
EPMAX=0. 
RDRX=RT 
NRUN=NRUN?#1 
WRITE (6,300) Ge RCVeRT»RDOTT yAGMTK pARTK 6S 0363) oNRUN 
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144 FORMAT(C” COOD. TURN SW =°13,% ROLS =7,F9034% COORD. TURN ROLL 
eee Se 3.) 


ROLDEG=ROL4*171850./P] 
ANADOT=CC-VXTRAXT-VZ2TRAZT)®ANAD/S(VH®VH) 
AZDOT=ANA/VH 
ELDOTIANE SV 
ROLDOT=10e7252*ANADOT/ (10.725 281067252 +ANA*ANA)D 
C V-TM = DELAYED TRUE Me,Y¥,2 VELOCITY 
VXTM=VXT 
VYTM=VYT 
VZTM=V2T 
C ~3 = XeY,Z DIFFERENCE BETWEEN RADAR AND TRUE TARGET POSITION 
X3=XR—-XT 
Y3=YR-YT 
Z3=2R=-77T 
9 RANGT = TRUE RANGE RANGTM = DELAYED TRUE RANGE 
RANGTM=RANGT 
RANGT=SQRTCX3axX3+yY3eV¥ 3473273) 
VNOIS=RNOIS (II) 
RANGN=RANGT?#VNOIS 
RANGN2Z=KANGT+RNOIS2Z(1]) 
REMDR=AMOD(RANGN,RES ) 
REMDRZ=AMOD (CRANGNZ2,RES) 
RESZ=RES/2. 
C RANGE = MEASURED RANGE WITH NOISE AND QUANTIZED TO RESOLUTION 
RANGE=RANGN-REMDR 
RANGE C=RANGN2-REMDR 
RT=RANGT 
& AMNL = MAGNITUDE OF NORMAL LOAD ACCELERATION ( 6°S } 
AL=13. 
BeEz-3, 
GAmM=0.5 
EPS=GGNOF CISEFED) 
AMNLZAL+BE* EXP CGAM*EPS) 
Az = AZDEG 
EL = -ELDEG 
ROLL = -ROLDEG 
PAUV(1) = X 
PAUV(2) = 2 
PAUV(3) = Y 
SAZ=SIN( AZ). 
CAZ=COSC A2 ) 
SEL=SIN(-EL) i 
CEL=HCOS(- EL ) 
SRO=SINC=ROLS) 
CRO=COS(-ROLS) 
TPACT)=PAUV (1) # CEL *CAZ-PAUV (2) *CEL*SAZ+PAUV (SD *SEL 


AGA AAA & 


¢ 


TPACZ)=PAUVC1) © (CCRORSAZ*SROFSEL*CAZI*PAUVI 2D CCROSCAZ—“SRO*SELASA: 


eo PAUVC3)*SRO*CEL 


TPAC 3) =PAUVC1) ®(CSRO*SAZ-CRORSEL*CAZI+FPAUV(I CI *CCRORSEL*SAZ+SRORCA. 


o*PAUVIS)*CROFCEL 
C ANUVC(T) = NORMAL ACCELERATION UNIT VECTOR 
= (TPACI) X V-TI/V CVECTOR CROSS PRODUCT) 
C (x,2,Y) 

ANUVOTIECOTPAC2ZI #VYT) -— CTPAC3) AVZTIIAV 


or 
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ANUVOZIVECCTPAC3)&VXT) = (CTPACT) eVYT))I/V 

ANUVOSISECCTPACT)#VZ2T) = CTPAC2ZIAVXT)IISV 
ANL (CI) = NORMAL LOAD ACCELERATION 

ANL (7) = ANUV(1)*® AMAL 

ANL (2) ANUV (2) ® AMAL 

ANL (3) ANUV (2) @AMNL 


ANLRCT) = RADIAL NORMAL LOAD ACCELERATION( 6°S ) 
= PROJECTION (DOT SCALAR PRODUCT) OF ANLSI) ON THE RADIAL 
VECTOR FROM TARGET TO RADAR/RADIAL RANGE 
ANE ROT) =CCANL (1) 9NX3)4 CANL 02) 423)4 CANLO3)*#Y3) I /RANGT 
YCT)=RANGE 
Y(2)=RANGE 2 
YC3)=aANLR (1) *6YD 
VR2D = FST DIFFERANCE, RANGE VELOCITY VR2D0 = DELAYED VR2D0 
VR2DD=VR2D 
VRZD=CRANGT-RANGTM)/(CDELT® FLOAT CNPRE) ) 
AR = RANGE ACCELERATION, 2ND OIFFERANCE 
ARM=AR , 
AR=(CVRZD0-VR2DDI/SCOELT* FLOAT (NPRE) ) 
/ WRITE(6,17133) RANGT,RANGTIM,VR2DD WROD AR,V(3),%(3), 
WX3,Y5073 
TEST RADIAL 2ND DIFFERENCE: AR, & LIMIT TO MAX, EXPECTED 
RATE OF CHANGE: SQRT(Q) 
ADIF=ABSCAR-AR™M) 
QLIM=SQRT(Q)*DT 
IFCADIF.GTeQLIM) AR=SIGNCQLIM,AR)+ARM 
TEST FOR RESINITILIZATION OF KALMAN FILTER 
SI6GTH=2.0 
SIGI=SIGTH*SQORT(R(1,1)) 
SIGZ=SIGTH*SQRTCR(2,2)) 
SIG3=AMAX1°S1IG1,S162) 
PMAX=O. 
EESTHAC CY C1) 4Y02))/22-)-x(1) 
IFCEEST eLT-SI63) 60 TO 44 
INCP=INCP 41 
IPS=7 
00 42 JP=15N 
PMAX=AMAXT(C PC JP IP)» PMAX) 
42 CONTINUE 
PPCT=1.0 
00 43 JP=1.4N 
P(JP,JP)=PMAX*PPCT 
43 CONTINUE 
44 CONTINUE 
WRITE (6,133) AZ yEL»sROL4S,TPAC1) pTPAC2) gg TPAC3) pANUV(T) pANUVOGO) » 
eANUV( 3), AMNL p> ANLAR (1) AR 
CALL FIKALI(K XH eGo VoSyQoR ePeINe IS eIL oN oMl oko TI eTlolTeT3, 
sTX,FG,ITER,IPS) 
T=T*O0T 
EPTCIP)= RT—K(1) 
XPTCIP)=XC1) 
VPTCIP)=XC2) 
VP2CIP)=VR2D 
APICIP)=xXC35) 
AP2CIP)=AR 
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AP3¢(1TP)=Y¥¢3) 
TPCIP)=T 
MTe CIP) =xtT 
ZTPCIP)=7T 
VPCT=X(C2)40T*®100./EPIC(IP) 
APCT=X C3) *DTe0TaSQO./EPICIP) 
PCTM=T2(1,1)*100./EPICIP) 
WRITE CG 4134) ITI ,VPCTAPCT yPCTMyT2(191) yPMAX 
134 FORMAT(1X,134° PCT VELOV, ACC, MEAS =7,10(F12.3)) 
IFCABSCEPTCIP)) GT. ABSCEPMAX)) EPMAX=EPITCIP) 
IFCABSCEP1CIP)).GT.40.) LL=LL*1 
K=K +1 
IF CFG(1,1).-GE.1-) GO TO 51 
WNS=FGC2,1)/(01.-F6(1,1)) *®DT) 
IFCwNSeLTLO2-) GO TO 51 
WN=SQRT(wWNS) 
WNHZ=wN/2-*P] 
OR=CFECTLIVI~“DT*FQC2,1)I/C 21 e-FGK1,1) 2D TtWN) 
194 FORMAT(” NATURAL FREQ. CHZ) =" 9F 902,” DAMPING RATIO =°.F8&.3) 
51 CONTINUE 
WRITEC6 4730) KoeRT,VC1) X01) X12) SEPICIP) sRNOIS(II) pAXTGAYTG AZT! 
®VR2D 
100 CONTINUE 


TOoPT(1)=1 

IOPT(5)=1 

NPP=NP+1 

CALL BEIUGRCEPT,NPP,IOPT,STAT,IER) 
WRITEC6,102) STATCT) ,STAT(S) ,EPMAX 

102 FORMAT( “* ARITHMETIC MEAN OF RANGE ERR =%,F7-3, 

*"s VARIANCE CAVE. RMS ERROR) OF RANGE ERROR ="yF 903, 
*°s MAXIMUM RANGE ERR =°,F9.,3,/) 

WRITE(6,104) LL,INCP 

104 FORMATOC/,” NO. OF RETURNS OUT OF GATE =",14, 

a” NO. TIMES P INCR.- =7,14) 

IRPLOT=1 

IFCIRPLOT.LEQ-0) GO TO 113 

CALL INTAXS 

CALL YAXANG(O.) 

CALL SIMPLX 

CAUM TITLEC” KALMAN FILTERS” ,100, 
* “ERROR IN YARDSS°, 

© 1O00,° TIME IN SEC.3% 4100460 9 8e) 

CALL XTICKS(S) 

CALL YTICKS(5S) 
XORG=AMINTCARMINCEPT.NP) sARMINCORNOI SoNP) ) 
XMAXGSAMAXTCARMAXCEPT,NP) ,ARMAXCRNOIS,NP) ) 
CALL GRAF CXORG, “SCALE ~ pXMAXG, 

* ARMIN(TP ,NP),”~SCALE” sARMAXCTP NPD) 
CALL CURVECEP1, TP,NP,10) 

CALL CURVECRNOIS,TP,NP,10) 

CALL MESSAG(°COV Q =$7%,100eT ee S89) 

CALL REALNO( Q,0,°ABRUT~,°“ ABUT” ) 

CALL MESSAG(° COV R =3°,100-¢ “ABUT” ,“ABUT~ ) 
CALL REALNO(R(1,1) 520 ABUT” ,°~ASUT”) 















2 


n 


GALL 
CALL 
CALL 
CALL 
CALL 
CALL 


MESSAG(® & =3°, 100, 
INTNOCNRUN, “ABUT”, “ABUT” ) 
DATE (WHEN) 

TOFDAYCTMEOD) 
MESSAG CWHEN, 10,1.,49. 3) 
MESSAGCTME0D.10,.°ABUT”,°“ABUT”) 


“APUT”, “ABUT” ) 


IDUMMY=LINEST(CIPAK,150,40) 


CALL LINESC"ERRORS”,1PAK,1) 
CALL LINES C°NOISES”% ,IPAK,2) 
CALL LEGENOCIPAK ,2,4.0,2.0) 
CALL ENDPL (0) 
CONTINUE 
IvVPLOT=0 
IFCIR .E€Q.22) IVPLOT=1 
IFCIVPLOT.FQ.0) 60 TO 114 
CALL INTAXS 
CALL YAXANG(O.) 
CALL SIMPLX 
CALL TITLE” KALMAN FILTERS” ,100, 
“ VELOC IN YARDOS/SECS”%, 
100.°TIME IN SEC.$%,100,606,8-e) 
CALL XTICKS(5) ; 
CALL YTICKS(5) 
XORGS=AMINICARMIN(CVP1,.NP) ,sARMIN( VP2,NP)) 
XAMAXG=AMAXTCARMAX(VP1,NP),ARMAX( VP2,NP)) 


CALL 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


GRAF (XORG, “SCALE” »XMAXG, 

ARMIN(TP ,NP),°SCALE~ gARMAX (TP 
CURVE (VP1, TP,NP,10) 
CURVE (VP2,TP,NP,10) 
MESSAG(°COV @Q =$~,100,1-, &-9) 
RE ALNO ( Q,0,°ABUT”,”“ ABUT”) 
MESSAG(” COV R =$°,100, “ABUT”, “ABUT” ) 
REALNO(R(1,1),2,°ASUT~ ,° ABUT”) 
MESSAG(° # =3°, 100, “ABUT”, “ABUT” ) 
INTNOCNRUN, “ASUT” » “ABUT” ) 
DATE (WHEN) 
TOFDAYC(TMEOD) 
MESSAG (WHEN, 105105903) 
MESSAG(TME0D.10,.° ABUT” »~ ABUT”) 


oNP)) 


IOUMMY=LINESTCIPAK,150,60) 


CALL LCINES(7X(2)87, J PAK,1) 

CALL LINES(°VR203$",IPAK,2) 

CALL LEGENDCIPAK ,2,5-0,1-0) 

CALL ENOPL(O) 

CONTINUE 

IAPLOT=1 

IFCIAPLOT.EQ.0) 60 TO 1148 

CALL INTAXS 

CALL YAXANG(O.) 

CALL SIMPLY 

CAEL TITLE C” KALMAN FILTERS” ,1C0, 
- ACCEL IN YDS/SEC*#SECS”, 

100,°TIME IN SEC.$%4100,64,8-) 
CALL XTICKS(S) 


CALL 


YTICKS(S) 





C 


XORG=AMINTCARMIN(CAP1,NP) ARMIN ( 
XMAXG=EAMAXTCARMAX(APTNP) ARMAX ( 


GAT L 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


CALL 
CALL 
CALL 
CALL 
CALL 


GRAF CKORG, “SCALE”, xXMAXG, 
ARMIN(TP 


CURVE CAP1, 


CURVE CAP2,TP.NP,1C) 
CURVE CAP3,TP .NP,10) 
MESSAG(’COV @Q@ 


REALNO ( 
MESSAG(” ¢ 


MESSAG(? & 


TP .NP,10) 


OV R 


=3°, 


100. 


=$°,100,1., 
@,0,°ARUT”, “ABUT” ) 
=#°,100, 


INTNOCNRUN,“ASUT’,“AKUT”) 


DATE (WHEN) 


TOFDAYCTMEOD) 
MESSAGCWHEN,10,106e9e23) 
MESSAG(CTIMEOD,10,°ABUT” ,° ABUT”) 
IDUMMY=LINESTCIPAK,150,&0) 
LINESC7xXC3)8$", IPAK,1) 
$°~,IPAK,2) 
LINES (C“Y(3)$°,1PAK,3) 
939420,2.0) 


LINES( "AR 


LEGENDCIPAK 
ENOPL (0) 


17148 CONTINUE 


a 
2 


® 


DRAW * AT RADAR LOCATION, 


CALL 
CALL 
CALL 


CALL TITLEC”° TARGET GROUND PLOT 


INTAXS 
VAXANG(OQ.) 
SIMPLX 


” OFF RANGE (72) 
”“ DOWN RANGE (X) 


CALL 
CALL 


XORG= 
XMAXG= 


XTICKS(5S) 
YTICKS (5) 


ARMINCZTP NP) 
ARMAX(ZTP,NP) 


= Yos.3$”°,100, 


aa YDS<%$. 5 100,66. 6) 


YMAXGZ=ARMAXCXTP,NP) 
CALL GRAF CXORG,”°SCALE” »-XMAXG, 


NPLS= 


ARMINCOXTP 


2 


CALL CURVECZTP.XTPeNP,SNPLS) 


CAEL 
CALL 


RDRY= 
IFOCRORX LES YMAXG) CALL CURVE CRORY ,RORX,1,1) 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


MARKER( 3) 
SCEPIC(2.) 
O. 


RESETC°SCLPIC”) 
RESETC “MARKER” ) 


MESSAG( “COV Q 


RE ALNO ¢ 
MESSAG(° 


MESSAG(” & 


XT=RT 


9 


=$°,100.1¢, 


APC,NP)) 
AP2.NP)) 


oNP).°~ SCALE” ,ARMAX (TP 


9) 


»NP)) 


KOU ome 7) 
REALNO(R(1,1),2,°ABUT” ,“ABUT”) 
“ABUT”, °“AEUT”) 


$°,1700, 


oNP),° SCALE” » YMAXG) 


ztT=0. 


&e9) 


Q.0,°aARUT”,°~ ABUT”) 
“ABUT” ,° ABUT” ) 
REALNO(R(1,1) 062, ABUT”, ABUT”) 
“ABUT” ,”“ ABUT” ) 


COV R 


=i 5 


=$°,100, 


100, 


INTNOCNRUN, ABUT”, “ABUT” ) 


OATE (WHEN) 


TOFDAYC(CTMEOD) 
MESSAGC WHEN, T0,1 00% 3) 








CALL MESSAG(TME0D,10.° ABUT” ,° ABUT”) 
CALL ENDPL(Q) 
116 CONTINUE 
118 CONTINUE 
115 CONTINUE 
IFCILAST.NE.1) GO TO 10 
CALL DONEPL 
120 FORMATCAX, 14 pA eg FO ol pe SFO 02) pOCFOT)D CCF 7.3)? 
130 FORMAT (4X,335 46F10.1),.76F10.3)) 
133 FORMAT(C1xX,74F9.2) 
150 FORMAT () 
END 
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C 


AAA AO 


SUBROUTINE INTCON 
INTCON INITILIZES TRAJECTORY VARIABLES 
DIMENSION PAUV(3) 
COMMON XT SVT sZTVXTSVYTSVZT VELOC oT TANG ey TZANG sDELRoIGyTRAT1, 
oDELTsNPRFETRAT2Z,I1INPH,ROO,IGZ2,1CTS Pl eIRsIHelOS 
COMMON/INICS AZTsEL yROLT,ROLZ,ROLS ,AZDEGsELDEGROLIDG ,ROL2ZDG, 
*ROL4SDG ,TEST»TES2,RT,RDOTT»R2D0TT, DT,TRAD1,TRADZ,RANGT, 
*RANGTMEXR YR eZRAVXTMAVYTM VZTMyVR2D pAXT AV Ty ALT sAXTGSAYTG,ALTG, 
SANA LANE,PAUY 
ITCS=1 
AzZ=0. 
EL=0. 
ROL1=0O. 
ROL2=0. 
ROL4=Q. 
AZDOEG=0. 
EL DEG=0. 
ROL1IDG=0. 
ROL2DG=0. 
TIANG=0. 
T6=0 
162=0 
DELT=DT 
NPRF=1 
3-D TARGET TRAJECTORY DATA 
ORIGIN AT INITIAL TARGET POSITION 
RT = TRUE RANGE RELATIVE TO RADAR 
RDOTT = TRUE VELOCITY RELATIVE TO RADAR 
R2DOTT = TRUE ACCELERATION RELATIVE TO RADAR 
IFCIREQ.1) RT=11275.6 
IFCIR.EQ.2) RT=1000. 
IFCIR.EQ.3) RT=150. 
IFCIOScEQe1) RDOTT=-175.6 
IFCIOS-EQ@e2) ROOTT=A-100. 
R2D0TT=0. 
VELOC = TARGET VELOCITY RELATIVE TO ORIGIN 
VELOC=-RDOTT 
TRAD= = TURN RADIUS OF TG6S— Ge TURN 
TRADIZVELOC#VELOC/TG6S1*10.7252 
TRADZ=VELOC*VELOC/TGS2*10.7252 
TRAT- = TURN RATE OF TGS- Ge TURN (RAD/SSEC) 
TRATI=TGSI#X1I0O.7252/ VELOC 
TRATZ=TES2X1TO0.7252/ VELOC 
RANGT=RT 
RANGTM=RANGT<-CRDOTT*#DELT*FLOAT CNPRE)) 
-R = RADAR LOCATION AT T(O) 
XR=ERT 
YR=0. 
ZR=0.6 
-T = TRUE X,Y,2 TARGET POSITION 
XT=0. 
YT=0. 
7T=0. 
V-7F = TRUE XeV¥,2 VELOCITY 
VXT=VELOC 
vYT=0. 
VZT=0. 
VXTMEVXT 
VYTM=VYT 
VZTM=v7ZT 


\) 
o>) 









AMAA 


VRCOG=RD 
A-T = 
Axer= (0), 
AYT=0. 
ATT=0. 
A-TG 
AxTG=0. 
AYTG=0. 
AZTG=O0. 
ANA = VERT 
ANE = HORI 
ANA=0,. 
ANE=0 
PAUV(]) 
PAUV(1) 
PAUV(2) 
PAUV(3) 
PAUV (1) 
PAUV(2) 


OTT 
TRUE XoY¥o2 ACCELERATION 


= TRUE X,Y,2 ACCELERATION (6°S) 


ICAL (AZ) ACCELERATION RELATIVE TO TARGET ORIENTATION 
ZONTAL CEL) ACCELERATION RELATIVE TO TARGET ORIENTATION 


PITCH AXIS UNIT VECTOR 
X 
Z 
Y 


=0. 


=) - 


PAUV(C3)=0. 


END 


Gd 








mec S55 MVR gk eMVAR SMVRT 


MANVR CALCULATES TRUE POS 
Myt.v¥Z1) TARGET STARTS @ 


LOCATION RT ON X AXIS 


PerKe TRUE VELOCITIES vA, 
NO FLIES TOWARG RADAR FIXED a 


x = DOWN RANGE, REFERENCED TO TAPCET ad T 7C 
Y = UP PANGE, REFERENCED TO TARGET @ T =e 
Z = OFF RANGE, REFESENCED TO TARGET 2 T =9 


SUBROUTINE MANVR 
GOCMHON AT,YT eZT »pVXTVYT VOT VELOC, TIANG, TZANG, DELR,ILE,TRAT:, 
oDELT»NPRF,TRATC,IIsNPH,ROC,IGZ2,ICTS,PI,I®,IH,LOS 
COMMON/INTC/ AZ,EL»eROLI,POLZ,RGOLY,AZHEG,ELDEG,FOL iDG ,~RO0L20G, 
mOOL4DG ,1TGS1,1TGS2,RT,RGOTT,R2O0TT, LT,TRAO1,TRADZ,SANGT, 
*RANGTM XR YR ZR aVXTMyVYTMy VZTM eV ROD pAXTyAYT AZT, AXTG AY TG ,ALTG, 
*ANA,ANE SPAUV 
*ROLGY ALONG X-AXIS ROTATES PITCH VECTOR CLOCKWISEs (FROM X,Ye2 = 
meet «6CdTOCUG pH A,e ) 
RR = ROLL RATE CF AIRCRAFT (CFEG/SEC) 
RRR = POLL RATE OF AIRCRAFT (RAD/SSEC) 
ICTS = COCROINATED TURN SwITCH 
DT=DELT 
RR=aGf. 
FRRIRK *PI/18C, 
CONDITIONAL 
feet ti wk ¥ eNPH) 
° ° INITIAL TRAJECTORY 
e e Pres =0 
e ae & PSX Te DELP 
» e ‘*MEASURED® AIRCRAFT POLL, PRECEEDING MANUVER 
© ee ROLY=ROLY*ERRR*XOT) 
meee oe F IN 
© (TILANGeLTeROO) 
e ° SECOND TRAJFCTORY 
© © ROLYZROLY*OCRRR*DOT) 
e e Meer OWN. Cle le55) 
e ° ° Otel. os 
® e steer Lt. 
meeee|)| CUI F (CI1G.CT.i8) 
e ° e ICTS=o 
sh wh lw! UTMMERSUREG® 6AIPCRAFT ROLL, PFECFECING MANUVER 
° e Py ROLYIZROLY-(RRAR*DT) 
e e erere rN 
e 9 1G6=(6%) 
ye TIANG = AZ ANGLE OF TAFGET 
~  e FTYANGZTRATISEDELTFFLOATINPRE) *IG 
Pees  X TEXT *tCELR*® COS (Ti ANC) 
° ° ZTIZT*OCELR*SINGT LANG) 
a. eeaicm 9 ¥xT = by VZF = VELOC 
MeV TEVELOCS COSC (TIANG) 
e ° VZTIVELOC*SINGUT2 AEG) 
6 arene LIN 
pets TUE « ) 


a5 


ae 





ees 86 CHIE Te ASE CTGRY 

;: ‘ emeeesUFET* AIRCRAFT PCLL 
° ° TF (FOL 4S e¢GTeU.C) 

° ° ° ROLYIRPOLY=<CRAR*OT ) 

® ° seer liv 

e s Peete Gel Teel ) 

° ° © FOLY=F,9 

r e aWemenh Lf 

mee §=66LGZ=1Ge+) 

° ° TEARGITRATS ©OLLTSFLOAT(NERFE)*1IGC 
° © YT=AYT*CELR*SINGTZANG) 

° ° ZTICT+*OCcLP*COStTCANG) 

° ° VYT=VELOC®S IN(TZANG) 

° . VZTIVELOC#COS(TCANG) 

e meer LN 

eeeF IN 

RETUPN 

END 
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C 


AAAAGCOA MA AA AA AYO AAON AAO AAA AAA AA AAA ANA AAAAA NAAN ANDANAAA AA AOAnaDA MO 


SUR ROUTINE 
C= F TKAL I 


n 


FUNCTION 
USAGE 


PARAMETERS 


_ = @ ae a & a 


FITK ALI 


IER 


(Ke A GHe Ga VeSe Ge Repbe lal 6 lt ON. T1572 


_ => = = <= 
ee “SF Bee eee eww eeweanwroewneae weer ewee ee @e eee @e@ = @ & @ @ & &@ & & & & & 


IT,T3,TX, TER) 


ITERATED KALMAN FILTERING 

CALL FTIKATIE CK pX gHeGoVpSeoQ gk oPyeINoIS oll oNoMil ol 
Te holiest sel he lERD 

INPUT STEP COUNTER: K=Ng1y2 ype 00 WHEN K=0, 
VECTOR X SHOULD CONTAIN THE PRIOR ESTIMATE 
OF THE MEAN OF xX, AND THE PROGRA™ 
CALCULATES THE ESTIMATED VARIANCE OF 
X AS P=GOG-TRANSPOSE AT STEP G. 

INPUT/OUTPUT VECTOR OF LENGTH Ne ON INPUT, 
X IS THE STATE VECTOR AT STEP Ky, AND ON 
OUTPUT, X CONTAINS THE ESTIMATED STATE 
VECTOR AT STEP K4#7. 

INPUT MATRIX OF DIMENSION N BY Ne H IS THE 
TRANSITION MATRIX AT STEP Ke 

INPUT MATRIX OF DIMENSION N BY L AT STEP K. 

INPUY OBSERVATION VECTOR OF LENGTH AT AT 
STEP K+1. 

INPUT MATRIX OF DIMENSION M14 BY N AT STEP K+" 

INPUT COVARIANCE MATRIX OF Use OF DIMENSION L 
AT STEP Ke 

INPUT COVARIANCE MATRIX OF Ve OF DIMENSION 
“1 BY MT AT STEP K41. 

INPUT/OUTPUT MATRIX OF DIMENSION N BY Neo 
ON INPUT, P IS THE VARIANCE MATRIX OF X 
AT STEP K. ON CUTPUT, P IS THE ESTIMATED 
VARIANCE MATRIX OF X AY STEP K+. 

FIRST DIMENSION OF HeG,» AND P AS DIMENSIONED 
IN CALLING PROGRAP, 

FIRST DIMENSION OF S AND R AS DIPENSTIONEDO IN 
CALLING PROGRAM, 

FIRST DIMENSION OF @ AS DIMENSIONED IN 
CALLING PROGRAM. 

INPUT. SEE DESCRIPTIONS OF X,~H-GoM,Pe 
N MUST BE GREATER THAN OQ. 

INPUT. SEE DESCRIPTIONS OF YoSeRoT Zin 
1 MUST BE GREATER THAN O.] 

INPUT. SEE DESCRIPTIONS OF G,Q@. 
tL MUST BE GREATER THAN QO] 

WORK MATRIX OF DIMENSION NM BY NM, WHERE 
NM 1S THE MAXIMUM OF N AND Mie 

wORK MATRIX OF DIMENSION N™ BY AML, WHERE 
NML IS THE MAXIMUM OF NM AND Le 

FIRST DIMENSION OF TT AND Fe AS 
DIMENSIONED IN CALLING PROGRAM, 

WORK WECTOR OF LENGTH 1. 

INPUT. ITERATED EXTENDED THRESHOLD 

ERROR PARAMETER. IER = O IMPLIES NO ERROR, 

TERMINAL ERROR = T12U4N 
N = 7 INDICATES ONF OF IN, IS, Ibe OR IT 

1S TOO SMALL. OR THAT ONE OF Ne M14 
Ok -L IS NOT A POSITIVE INTEGER. 


N77 





> eile. 8 





Oy a ey 


Ce OY CY Cites 


N = 2 JNDICATES SATFIXN INVERSION FAILED. 


PRECISION - SINGLE 
REG™D IMSL ROUTINES - LEQTIFLUOATF LUELMF LUERTST,V¥ULFF ye VMULFP 
LANGUAGE - FORTRAN 
LATEST REVISION - MAY 10, 1479 
MODEL 
x(n 417) HOCK) *xX0K)4+G60K )#UCK) 


Y¥(K+1) SOK +Tax (K+1 04 (K 41) 
SUBROUTINE FTKALI (KEM GHeGe Vocus Gg ReuretnelSeli. 
* NoMT Lol TeteselieoligtTAeFGeITER ZIPS) 
DIMENSION . XO1) sHOIN 9 4) GOING 1) eV 612 6 SCES oI) oe GCIL oT)» 
* ROIS, TI ePCIN GTI cTICIT eI «sTOCIT eID. T3C1) 
DIMENSION XK (20), XPCZO),EXC 20), TXCIN) -T4°020) —HNL(I2C), 


*Ex*(20),FGC1T.1) 

DIMENSION 1715¢20).76(10,10) 

DATA 1O0/0/,11/1/,120/20/ 

IF CINeGE CN ecANDow IS 26GEM1 CAND. IL GEL 
a e AND. CITeGE.N eOR. I7T-GEem1) eANDe Ne67T.0 
* oAND. M1.G720 eANDe L-GT-0) GO TO S 


TER = 129 
GO TO 9000 
5 IER = O 


CALCULATE P IF K = ZERO 
P(O0) = 6(0)*0Q(0)*G6T(0) 
IF (K NE’ OF 60 TO 10 
CALL VMUL FF ( G, Q.» N. Le LetNae Tete ell ere RD 
CALL VMULFP (T2, Ge Ny Lo NeolTeINe PeoINgIER)D 
IFC IPS.EQ.0) 60 TO 166 
WRITE (6,160) €(7P(1,53) -JETe¢INI  CTXCI) -I=1,IN) 
DO 165 JT=2,I1N 
WRITEC6,1671) (PCI,J) eI=TVZIND 
165 CONTINUE 
166 CONTINUE 
160 FORMATC(” P(0O) =%,10(0€&10.4)) 
1671 FORMAT(§&xX,100E1024)) 
10 CONTINUE 
SAVE INITIAL X 
DO 12 J=1,N 
XK CT) =xXCT) 
12 CONTINUE 
CALCULATE xX-PRIME AT STEP K+1 
X7(K41) = HOCK) *#X-HAT CK) 
CALL VMULFF CHe Xo Ny NoIITeINgINoTIZITA,IER)D 
00 15 I = 1,N 
xP(J) = TICI,17) 
XCY)I=KPC]) 
15 CONTINUE 
IFCIPSSEQ.1) WRITECO6170) (XPCI)D I=IN) 
170 FORMAT(” X-PRIME = HeX =°,700F10.2)) 
CALCULATE P-PRIME AT STEP xk*1 
P7(K 47) = HOCK) *®P CK) AHI OK DAG CK) 800K) 8 GT (KK) 
CALL VMULFF ( Hey Poe Ne No NoINeINeT IT61F eIER) 


gy 


CG 


C 
C 


E 


20 
25 


frs'S 
186 
180 
181 


CALL VMULFP (T1, Hy Ny Ny NI 
CALL VBULFF ( 6G, Qe Ny Leo Ly! 


TolN, PreIN, TER) 
NoIL sTcoT TIER) 


CALL VMULFP (Te, Gy, Ne Ee Ne lTeINe It Vell. eR?) 


50 25 1 = 1,N 

DO 20 jg = 1.N 

PCI,J) = PCI,s) * THC, 

CONTINUE 
CONTINUE 
IFCIPS.~EQ.0) GO TO 186 
WRITEC6,180) (P(1,5),J=1,IN) 
00 185 I=2,IN 
WRITE(6,781) (PC1,J5),S=15IN) 
CONTINUE 
CONTINUE 
FORMAT(” P=-PRIME =°,10(E10.4 
FORMATC11X,100F10.4)) 


J) 


») 


CALCULATE MATRIX K AT STEP K*1 


KOK4+T) = PO CKFDI STOCKH ROS CK41) POCKET AST OK 41) OR CK 41) ) WT 


ef 
28 


30 


5) 
187 


40 


45 
50 


193 
194 
190 
191 


CALL VMULFP ( S, PoMTt, Ny Ny! 
IFCIPS.EQ.0) GO TO 28 
00 27 r=1,m1 
WRITEC6,787) I,4(T201,55) -SJ=1 
CONTINUE 
CONTINUE 
CALL VMULFP CT2, SoM1, No M1,/] 
00 35 I = 1,™1 

DO 30 J = 1,1 

TICI J) = TICI,J) © RCS 

CONTINUE 
IFCIPS.EQe7) wRITEC6,787) I1,€ 
CONTINUE 
FORMATC” SoPeST#R( "9,11," 5J) 


SeolN,T2eIT,IER) 


oM1) 


TeISSTITITSZIER)D 


1) 
TI1CT, 33) pS I=1 M1) 


= 71 =°,10(F10.2)) 


CALL LEQTIFCT1, NeMI,IT,T2  TO,T3,IER) 


IF CIER -~-FE@. OP GO TO 40 
IER = 130 
60 TO 9000 
00 50 I = 1,1 
00 48S J = 14N 
oo ee = T2¢],J) 
FOC S eI) =TICI,I1) 
CONTINUE 
CONTINUE 
IFCIPS.EQ.0) GO TO 194 
WRITE(6,790) (CT101,1),1=1,81) 
00 193 I=2,IN 
WRITE(6,191) CTICIT I), J=1 m1) 
CONTINUE 
CONTINUE 
FORMAT(” K = T1 =°,100F10.2) 
FORMAT(10xX,1710¢0F10.2)) 


C KALMAN ESTIMATE 
X-HAT(CK*1) = 17 OK) -K OK O1)9* 0S OK +1 


£ 


KL=0 
60 53 T=1-IN 


) 
CALCULATE X-HAT AT STEP K1 


yax? (kK )-V¥ CK 4+1)) 





Oo 


53 
54 


Dhe 


eek = () 4 
ExXMCI)D=1FS9 
CONTINUE 
CONTINUE 
00 $542 I=1,1N 
TOCTI=EXP(1)-x¢1) 
CONTINUE 


OBS. ERR. = SOCX7=-X)-Y*HNL = 15 


22 


60 


195 
T1 


oi, 


200 
69 


CRIES VME F € S45 XeM4g Nelly ISeINol 3011S elER) 
CALL VMULFF ( S,T4eM1, Noelle ISoINeTSelSe IER) 
CALL VMULFFC Sy, XM1, Nelle IS,INSHNL eg IZC IER) 
00 60 I = 1,1 

T3CI) = 1341) - YC) 
CONTINUE 
IF CIPS.EQ@.1) wRITE(6,195) CT3C1),1=10™1) (X01) gL=19N) 
FORMAT(C” OBS ERR = SaxX-¥ =°,120F9e2)) 
= GAIN K 
CALL VMULFF (T1.T3, NeMl oll eIT»s1S estTl2elTe IER) 
00 65 I = 1,N 

XCI) = xP(]) = T2¢],1) 
T6O(1.7)2=T2¢01,1) 
CONTINUE 
IF CIPS 2EQ.91) WRITEC6,200) (XCI) THT IN) SC T2061 57) -3=1,N) 
FORMAT(% X-HAT =°,10(F10.22)) 
CONTINUE 

CALCULATE P AT STEP x1 


P(K41) = P7 (KOT) —-K (KOT) 25 CK 41) «P% CK 41) 


70 
75 


215 
210 


9000 


9005 
120 
130 
140 
210 
on 


CALL VMULFF CT1, Se NoeM1, NeoelTeIS es¥2eiTeIER) 
CALL VMULFF (T2, Py Ny No NeolToINoeTIeI TIER) 
00 75 I = 1,N 
T2C0T21)=T601,1) 

00 70 J = 1.6N 

P(I,J) = P(1,J) a TIC] ,J) 

CONTINUE 
CONTINUE 
1FCIPS.~EQ@.0) GO TO 216 
WRITEC6,210) (P(1,3),3=1,1N) 
00 215 1=2,I1N 
WRITE(6,211) CPC] J) -J=T,ZIND 
CONTINUE 
CONTINUE 
60 TO 9005 
CONTINUE 
CALL UERTST CIER,O6HFTKALM™) 
RETURN 
FORMATC AUX, 14 IX oF OeT IOC FI1.2)) 
FORMAT(1X,134120F10.3)) 
FORMATC2X,13,120F10.2)) 
FORMAT(” P =7,10¢F10.2)) 
FORMAT( 5x,10¢0F10.2)) 
END 
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